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ABSTRACT 


Much  of  the  current  state  of  the  art  technology  for  the 
dynamics  and  control  of  robotic  manipulators  is  geared 
primarily  towards  \  industrial  applications.  Industrial 
manipulators  usually\operate  in  well  structured  environments, 
i.e.  mounted  on  fixed  bases  working  with  known  parameters 
such  as  payloads, etc . ^Mounting  robotic  manipulators  on  moving 
bases  offers  the  potential  of  greatly  extending  the 
manipulator  applications.  However,  the  motion  of  the  base 
creates  a  number  of  problems  in  terms  of  the  control  of  the 
manipulator.  The  base  motion  subjects  the  manipulator  to 
large  scale  disturbances  that  can  seriously  degrade  system 
performance.  This  thesis  illustrates  the  problems  associated 
with  a  moving  base  as  it  applies  to  a  two  degree  of  freedom 
manipulator.  It  is  shown  that  conventional  control 
techniques,  such  as  PID  control,  cannot  effectively  compensate 
for  the  base  accelerations  in  a  typical  application.  It  is 
also  shown  that  more  advanced  control  algorithms,  including 
decoupling  control,  LQR  control,  and  pole  placement  prove  to 
be  ineffective  to  varying  degrees.  The  linear  controller  that 
had  the  best  results,  the  one  designed  utilizing  a  pole 
placement  technique,  is  then  applied  to  the  complete  nonlinear 
system  in  an  attempt  to  invescigate  the  influence  of  the  base 
motion  over  the  entire  range  of  motion  of  the  manipulator. 
Recommendations  for  possible  solutions  to  the  moving  base, 
problem,  along  with  recommendations  for  extensions  of  this 
work,  are  then  made.  _ „ 
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CHAPTER  1 
INTRODUCTION 

1.1  Introduction 

Robotics  is  a  technology  that  is  moving  into  many  realms 
of  today's  society,  particularly  in  the  area  of  industrial 
manufacturing.  State-of-the-art  technology  allows  us  to  build 
and  program  robotic  devices  and  manipulators  that  can  perform 
many  of  the  tasks  that  are  currently  being  accomplished  by 
human  operators.  This  has  proven  to  be  extremely  useful  in 
those  tasks  that  are  inherently  dangerous  to  human  operators, 
and  those  that  are  mundane  and  repetitive. 

In  addition  to  the  obvious  industrial  applications, 
robotics  has  a  role  to  play  in  today's  military.  In  fact,  a 
study  by  a  committee  appointed  by  the  National  Academies  of 
Science  and  Engineering  in  the  early  1980 's  identified 
numerous  military  tasks  that  could  be  performed  by  robotic 
systems  till.  These  included  a  tank  autoloader,  a  robotic 
sentry,  and  various  ammunition  handiincr  devices.  A  basic 
problem  is  the  fact  that  the  technology  that  enables  us  to 
build  industrial  manipulators  is  not  directly  transf errable  to 
manipulators  that  would  operate  in  a  military  environment. 

Today  s  industrial  manipulators  are  designed.  to  wot k 

under  highly  structured  and  "friendly"  conditions.  They  are 
mounted  on  stationary,  relatively  rigid  bases.  They  are 
programed  to  complete  highly  specif  iced  tasks  in  which  most 
often  the  parameters  (particularly  the  payload  parameters)  are 
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relatively  well  known  C2J.  In  contrast,  manipulators  in 
military  applications  would  be  forced  to  work  in  a  "hostile" 
dynamic  environment  in  which  many  critical  system  parameters 
would  be  constantly  changing.  In  order  to  accomplish  many 
military  missions,  the  robotic  manipulator  would  have  to  be 
mounted  on  a  moving  base.  Moving  bases  would  enable 
manipulators  to  be  transported  throughout  their  areas  of 
operation.  The  utility  of  these  systems  could  be  maximized  if 
the  manipulator  could  operate  both  while  the  base  is  moving 
and  after  the  base  arrives  at  the  new  location. 

Two  relatively  simple  prospective  examples  which  would 
involve  manipulators  mounted  on  moving  bases  are  a  tank 
autoloader  and  a  manipulator  mounted  on  the  back  of  a  truck  to 
handle  ammunition.  The  tank  autoloader  would  be  used  to  load 
main  gun  rounds  in  the  gun  tube  of  a  tank.  The  manipulator 
mounted  on  the  back  of  a  truck  would  perform  such  tasks  as 
picking  up  pallets  of  ammunition,  breaking  open  the  pallets, 
and  repackaaing  the  ammunition  into  smaller,  user  oriented 
packages.  There  is  an  important  technical  difference  between 
these  two  applications  that  must  be  addressed.  In  the  tank 
autoloader  problem  the  motion  of  the  manipulator  would  have  no 
effect;  on  the  tank  itself,  orimarily  due  to  the  mass 
difference  between  the  two  ob  i«t:cs .  uri  the  ocner  nano,  tne 
motion  of  the  manipulator  mounted  on  the  back  of  a  truck  would 
cause  the  truck  itself  to  move,  which  would  create  additional 
dynamic  problems. 
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In  such  military  applications  these  moving  bases  would 
possess  significant  suspension  flexibility  that  could 
seriously  degrade  system  performance.  Although  outriggers 
could  possibly  be  used  to  reduce  this  base  flexibility,  they 
would  probably  not  be  sufficient  to  negate  all  flexibility 
problems.  When  the  vehicle  or  base  is  in  a  transportation 
mode,  the  manipulator  would  be  subject  to  large  dynamic 
disturbance  forces  that  would  also  tend  to  degrade 
performance.  In  addition  this  base  motion  would  excite  the 
structural  resonances  of  the  manipulator  causing  additional 
problems.  Another  major  problem  facing  manipulators  working 
in  a  military  environment  would  be  the  large  payload  range 
that  the  manipulators  might  need  to  handle.  For  example,  if  a 
manipulator  was  to  be  used  as  an  automatic  loader  for  a  tank, 
the  weight  of  the  payload  would  vary  from  forty  to  sixty 
pounds  depending  upon  the  type  of  shell.  These  payload 
variations  would  also  degrade  performance. 

In  order  to  bring  robotic  technology  to  the  point  where 
robotic  manipulators  can  be  used  in  a  military  environment, 
the  above  problems,  in  addition  to  others,  need  to  be  solved. 
This  thesis  focuses  on  some  of  the  dynamics  and  control 
problems  encountereu  when  the  manipulator  is  mountca  on  a 
moving  base.  The  disturbances  that  the  manipulator  is  subject, 
to  as  a  result  of  the  base  motion  are  studied,  and  various 
control  strategies  are  investigated  to  determine  how  well  they 
compensate  for  the  base  disturbances. 


In  order  to  fully  understand  the  dynamics  and  control 
problems  of  mobile  manipulators,  it  is  essential  to  first 
determine  what  effects  large  base  motions  have  on  their 
performance.  This  is  done  by  using  dynamic  simulations.  To 
conduct  meaningful  simulations  which  include  the  dynamic 
nonlinearities , a  system  with  realistic  characteristics  is 
designed.  The  model  was  designed  with  a  realistic  application 
in  mind,  that  of  the  tank  autoloader.  Payload  parameters, 
actuator  restrictions,  cycle  time  and  geometric  constraints 
all  were  considered.  For  the  purpose  of  this  research  a  two 
link,  two  degree  of  freedom  manipulator  was  modelled  by 
simulation.  The  manipulator  links  were  assumed  to  be  rigid. 
Only  planar  motion  was  considered. 

It  is  important  at  this  point  to  understand  the  tasks 
that  a  robotic  manipulator  serving  as  a  tank  autoloader  would 
have  to  perform.  Figure  1-1  shows  the  current  tank  loader,  a 
human  operator  L303.  The  loader  stands  inside  the  tank 
turret,  and  during  the  loading  sequence  he  reaches  to  the  rear 
to  extract  a  round  out  of  the  ammunition  storage  area  (ready 
rack).  He  then  pivots  and  loads  the  round  into  the  gun  tube. 
A  manipulator  desianed  to  perform  this  mission  would  operate 
in  the  same  mode  in  basically  the  same  workspace. 


FIGURE  1-1  HUMAN  OPERATOR  AS  TANK  LOADER 


Once  the  system  for  the  autoloader  application  was 
designed,  the  nonlinear  equations  of  motion  were  developed 
using  the  Lagrangian  technique.  A  nonlinear  simulation  was 
then  conducted  in  order  to  determine  the  extent  of  the 
problem.  In  addition,  a  nonlinear  simulation  at  this  point  in 
the  analysis  provided  for  an  opportunity  to  check  the  response 
of  the  modelled  system  acrainst.  anticiDated  response  based  on 
enaineerina  experience. 

The  nonlinear  equations  of  motion  were  then  linearized 
around  selected  nominal  values.  This  was  done  to  provide  for 
linearized  equations  of  motion  for  which  linear  controllers 
could  be  designed.  Analysis  of  linearized  equations  allowed 
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for  investigation  of  the  system  without  nonlinear 
complications.  The  linear  controllers  designed  were  then,  by 
definition,  effective  for  small  perturbations  around  the 
selected  nominal  values. 

Conventional  control  strategies  were  then  applied  to  the 
linearized  syscem  to  study  their  effectiveness  in  compensating 
for  the  base  motion.  In  the  analysis,  the  system  was  first 
modelled  as  a  manipulator  being  mounted  on  a  fixed  base.  A 
controller  was  then  designed  that  met  operating 
specifications.  The  model  was  then  changed  to  provide  for 
motion  of  the  base,  and  the  linear  controller  that  had  proved 
to  be  effective  for  the  fixed  base  problem  was  then  applied  to 
the  system  with  base  motion.  A  PID  controller  was  first 
developed  and  investigated,  but  it  was  unable  to  effectively 
compensate  for  the  motion  of  the  base.  More  advanced  control 
algorithms  were  then  applied  to  the  problem,  using  the  same 
procedure  described  above.  They  also  proved  to  be  ineffective 
in  compensating  for  the  base  motion,  although  some  had  more 
success  than  the  others.  The  advanced  control  algorithms 
studied  included  decoupling  control,  LQR  control,  and  a  pole 
placement  technique. 

At  this  point  the  linear  controller  that  had  the  best 

results  was  then  applied  to  the  full  nonlinear  system  to  study 
it's  effectiveness  over  the  entire  range  of  motion  of  the 
manipulator,  taking  into  account,  all  of  the  nonlinear 

complications.  As  with  the  linear  control  design,  the 
controllers  effectiveness  was  first  studied  as  applied  to  a 
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manipulator  in  the  absence  of  base  motion,  and  then  base 
motion  was  added  to  the  system.  It  was  found  that  the 
controller  was  indeed  effective  in  the  absence  of  base  motion, 
but  was  unable  to  compensate  for  the  motion  of  the  base. 

1.3  Detailed  description  of  the  moving  base  problem 

The  motion  of  the  base  creates  many  dynamics  and  control 
problems  for  the  manipulator.  The  large  inertial  disturbance 
forces  that  are  generated  by  the  motion  of  the  base  create  a 
hostile  dynamic  environment  for  the  manipulator.  These 
dynamic  disturbances  are  similar  to  those  one  feels  when  he  is 
in  an  elevator  while  the  elevator  accelerates  or  decelerates. 
Even  though  the  elevator  control  panel  and  the  individual  are 
moving  in  the  same  reference  frame,  an  individual  has  a 
certain  degree  of  difficulty  reaching  out  to  push  a  control 
button  while  the  elevator  is  starting  or  stopping.  Another 
good  example  would  be  the  disturbances  one  feels  when  he  is  in 
an  automobile  traveling  down  a  bumpy  road.  Reaching  out  to 
pull  out  the  cigarette  lighter  or  turn  the  radio  on  can  become 
difficult  as  a  result  of  the  base  motion.  In  addition,  the 
motion  of  the  base  could  excite  the  manipulator's  structural 
resonances.  This  could  severely  degrade  system  performance. 

The  results  shown  in  this  thesis  indicate  that 
conventional  control  techniques  cannot  effectively  control  a 

manipulator  mounted  on  a  moving  base.  In  addition  it  is  shown 


that  more  advanced  control  algorithms  are  also  ineffective. 
It  is  suggested  that  effective  controllers  could  possibly  be 
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designed  if  the  controller  had  access  to  sensory  information 
that  would  indicate  the  motion  of  the  base.  This  sensory 
information  could  be  provided  by  sophisticated  linear 
accelerometers  capable  of  an  extremely  high  sampling  rate. 
The  base  velocity  could  then  be  determined  by  integration 
routines . 

1.4  Description  of  status  of  current  research 

Research  in  the  realm  of  robotic  devices  and  manipulators 
to  date  has  been  focused  on  design  and  control  of  industrial 
robots.  As  a  result,  the  "moving  base"  problem  has  recieved 
little  attention.  Soviet  engineers  are  investigating  the 
problem  to  a  certain  degree,  however.  They  have  recently 
focused  on  the  problem  of  how  to  mathematically  model  a 
manipulator  mounted  on  a  moving  platform.  Their  published 
work  to  date  has  focused  on  modelling  considerations  and  does 
not  address  the  issue  of  appropriate  control  stategies  C33. 

A  larue  amount  of  current  research  reaarding  the  dynamics 
and  control  of  robotic  manipulators  can  indirectly  be  applied 
to  the  "moving  base"  issue,  however.  The  Luh-Walker-Paul 

algorithm  that  is  designed  to  compute  the  forces  and  toraues 
necessary  to  control  a  manipulator  along  a  planned  path 

provides  for  the  inclusion  of  base  motion  in  the  computation 
of  required  torques  C43.  The  required  torques  can  then  be 
dynamically  fed  forward  (inverse  dynamics)  in  an  attempt  to 
compensate  for  the  motion  of  the  base.  Cannon's  work  on  end 
point  control  could  be  applied  to  the  "moving  base"  issue  by 
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focusing  on  the  exact  location  of  the  end  point  in  relation  to 
the  target  regardless  of  the  base  motion  C5U. 

In  addition,  there  has  been  work  in  areas  that  are 
peripheral  to  the  problem  of  mounting  a  manipulator  on  a 
moving  base.  Research  is  being  done  at  the  Rensselaer 
Polytechnic  Institute  in  regards  to  operating  a  robot  in 
constrained  work  volume,  as  would  be  the  problem  with  a  tank 
autoloader.  This  work  concentrates  on  the  trajectory  problem 
that  is  a  function  of  the  control  strategy  selected  C273. 
Researchers  at  the  University  of  Iowa  are  concentrating  on 
techniques  to  model  the  dynamics  of  a  vehicle  with  flexible 
and  rigid  components  intermixed.  This  research  illustrates 
what  effect  bumpy  terrain  has  on  the  vibrating  motion  of  a 
vehicle,  which  in  turn  would  effect  the  manipulator  mounted  on 
the  vehicle  C283.  In  the  area  of  military  vehicles,  models 
have  been  developed  that  predict  dynamic  track  loads  on 
vehicles  that  navigate  cross-country  C29J. 

In  summary,  research  in  the  area  of  the  dynamics  and 
control  of  a  robotic  manipulator  mounted  on  a  moving  base  is 
sparse  at  best.  Little  has  been  done  to  date  to  determine 
what  effect  base  motion  has  on  the  control  of  the  manipulator, 
and  whether  or  not  conventional  control  algorithms  can 
effectivly  compensate  for  the  base  motion. 
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CHAPTER  2 

DETERMINATION  OF  MODEL  PARAMETERS 

2.1  Introduction 

It  is  imperative  in  investigating  the  problems  associated 
with  the  dynamics  and  control  of  a  robotic  manipulator  mounted 
on  a  moving  platform  that  the  researcher  consider  realistic 
applications.  As  was  mentioned  before,  there  exists  numerous 
potential  applications  for  the  mobile  robot  within  the 
military.  This  thesis  will  focus  on  the  concept  of  mounting  a 
robotic  manipulator  inside  a  tank  to  load  ammunition.  This 
will  allow  for  selection  of  realistic  model  parameters  in 
addition  to  identifying  acceptable  performance  specifications. 

1.2  Design  of  Model 

Modeling  is  a  critical  aspect  of  engineering  analysis.  A 
designer  can  take  an  extremely  complex  real  system  and, 
utilizing  appropriate  design  procedures,  develop  a  mode'.  that 
will  emulate  the  performance  of  the  actual  system.  It  is 
important  to  design  the  model  to  be  as  simple  as  possible  by 
making  certain  simpiifyincr  assumptions  so  tnat  the  significant 
system  behavior  can  be  analyzed.  However,  the  model  must 
clearly  represent  the  behavior  of  the  actual  system,  and  avoid 
oversimplifications  which  result  in  misleading  conclusions. 
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The  model  used  for  this  research  was  a  two  degree  of 
freedom,  two  link  planar  manipulator.  The  design  of  the  model 
is  as  indicated  in  figure  2-1: 


Certain  assumptions  are  made  in  order  to  insure  that  the 
model  was  a  simple  as  possible  but  still  emulated  the 
important  actual  system  behavior.  These  included: 

(1)  The  motion  of  the  manipulator  is  restricted  to  planar 


motion . 
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(2)  The  manipulator  is  rigidly  fixed  to  its  moving  base. 

(3)  The  links  of  the  manipulator  are  assumed  to  be  thin 
walled  hollow  steel  tubes.  The  center  of  mass  of  the  links  is 
located  along  the  link  center  line.  The  links  are  assumed  to 
be  rigid. 

(4)  The  actuators  and  end  effector  are  included  in  the 
model  for  mass  distribution  purposes  only.  All  motor  backlash 
effects,  etc.  were  neglected. 

In  order  to  determine  model  parameters,  the  use  of  the 
system  had  to  be  considered.  The  fact  that  this  design  was 
intended  as  a  tank  autoloader  resulted  in  the  following 
assumptions : 

(1)  The  payload  that  this  manipulator  would  handle  would 
vary  from  forty  to  sixty  pounds. 

(2)  The  work  space  dimensions  and  hence  the  size  ot  the 
links  for  the  manipulator  are  consistent  with  the  size  of  the 
tank  turret.  These  dimensions  are  indicated  in  the  followina 
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ammunition  storage  rack  (inside  turret) 


(inside  turret) 

FIGURE  2-2  MANIPULATOR  WORKSPACE  DIMENSIONS 

This  workspace  dictates  that  the  height  of  the 
manipulator  when  in  a  vertical  position  can  be  no  more  than 
seven  feet.  In  addition,  the  manipulator  trajectory  must  be 
able  to  include  the  path  from  the  ammunition  rack  to  the  gun 

cube . 


At  this  point  the  system  parameters  can  be  determined. 
This  is  done  in  the  following  manner: 

STEF  1  -  MASS  PARAMETERS 

The  thickness  of  the  walls  of  the  thin  walled  hollow  steel 
tube  forming  the  links  is  dictated  by  the  assumption  that  the 
links  would  be  effectively  rigid.  The  natural  frequncies  of 
the  links  must  be  high  enough  so  that  the  motion  of  the  base 
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does  not  excite  them,  and  the  structural  resonances  of  th 
links  do  not  interact  with  the  manipulator's  control  system 
The  link  parameters  were  calculated  in  the  following  manner: 

link  1 


R=4. 5" 
r=3 . 5 " 

L  =36" 
1 


link  2 


FIGURE  2-3  MODELS  FOR  LINKS 


R=4 . 0 " 
r=3 . 0 " 
L  =  36" 
2 


2  2 

Volume  Calculations:  V  =  RL-rL  (2.1) 


3 

V  =  904.76  in 

1 

3 

V  =  791.70  in 

2 


3 

Density  of  steel  =  .284  lb/in 


Therefore: 
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Weight  link  1=  904.76  x  .284  =  257.0  lbs 
Weicht  Link  2=  791.7  x  .284  =  224.8  lbs 

The  motor  actuators  have  to  be  included  in  order  to 
include  all  of  the  mass  elements  to  locate  the  link's  center 
of  mass.  Due  to  the  relatively  heavy  payloads,  it  was  assumed 
that  hydraulic  rotary  actuators  would  be  used.  A  study  of 
commercially  available  rotary  actuators  capable  of  performing 
designated  tasks  determined  that  suppliers  such  as 
Bird-Johnson  marketed  actuators  that  would  suffice  C333. 
These  Bird-Johnson  actuators  are  Model  HYD-RO-AC,  and  have  the 
following  representative  characteristics: 

ACTUATOR  1=  80  lbs,  max  torque  20,000  in-lbs 

ACTUATOR  2=  30  lbs,  max  torque  5,000  in- lbs. In  terms  of 
mass  distribution  it  was  assumed  that  the  weight  of  the 
actuators  would  be  distributed  throughout  the  links  in  the 
following  manner: 

Weight  of  actuator  1:  75%  on  platform,  25%  on  the  lower 

end  of  link  1 

Weight  of  actuator  2:  6o%  on  link  1,  37%  on  the  lower 

end  of  link  2 

In  addition,  a  commericially  available  end  effector  that 
would  handle  the  prescribed  payloads  was  found  that  weighed  25 
lbs.  The  end  effector  will  be  mounted  as  shown  in  figure  2-1. 
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Based  on  the  above  calculations  and  assumptions,  mass 
calculations  can  now  be  made: 

MASS  of  link  1=  total  weight  on  link  1/acceleration  due 
to  gravity 

=  297/32.2  =  9.2  slugs 

MASS  of  link  2  (with  601b  payload) =  total  weight  on  link 
2  +  payload/  acceleration  due  to  gravity 

=  319.8  /  32.2  =  9.93  slugs 

MASS  of  link  2  (without  payload )=  total  weight  on  link  2/ 
acceleration  due  to  gravity 

=  259.8  /  32.2  =8.07  slugs 

STEP  2-  CALCULATON  OF  CENTER  OF  MASS  LOCATIONS 

The  location  of  the  center  of  mass  for  each  link  can  be 

determined  by  using  the  following  formula: 


1  -  -  (2.2) 

ic  m 

i 


where : 

m  =  mass  of  each  link  (including  actuators,  etc.) 
i 


actuator  2 

(37?  on  link  2) 


FIGURE  2-4  MASS  DISTRIBUTION  FOR  LINKS 

Which  leads  to  the  following  parameters: 
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1  =  18.4" 

lc 

1  =  22.6"  (with  payload) 

2c 

1  =  15.7"  (without  payload) 

2c 


STEP  3-  CALCULATION  OF  MASS  MOMENT  OF  INERTIA 


The  mass  moment  of  inertia  can  be  thought  of  as  a  measure 
of  the  link's  resistance  to  rotational  motion.  It  is 
evaluated  about  an  axis  passing  through  the  object's  center  of 
gravity.  The  values  for  the  moment  of  inertia  about  each  link 
are  as  follows: 

2 

J  =  6.44  lbf-sec  /  ft 

link  1 

2 

J  =  5.11  lbf-sec  /  ft 

link  2 


All  calculated  parameters  are  presented  in  the  following 
table : 


mass  link  length  center  of  mass  m.o.i. 

(m)  (L)  (1)  (J) 

slugs  feet  feet  lbf-sec  /ft 


link  1 

link  2 
( wi  th 

payload ) 

link  2 
(w/o 

payload) 


9.2 

3 

1.53 

6.44 

9.93 

3 

1.88 

5.11 

8.07 

3 

1.31 

.  5.11 

FIGURE  2-5  TABLE  OF  MODEL  PARAMETERS 
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2.3  Model  for  base  acceleration 


It  is  also  important  to  effectively  model  base 
accelerations.  The  base  accelerations  that  the  model  is 
subjected  to  must  closely  simulate  the  type  of  accelerations 
that  tank  moving  cross  country  would  experience.  Due  to  the 
irregularity  of  ground  conditions,  combined  with  the  high  rate 
of  speed  of  today's  tanks  (45  mph) ,  the  tank  would  be  subject 
to  random  input  accelerations  of  relatively  high  magnitude. 
For  the  purposes  of  this  thesis  only  vertical  accelerations 
will  be  considered.  Past  experiments  have  indicated  that 

maximum  vertical  accelerations  would  be  of  the  order  of  16 

.  2 
I  ft/sec  ,  approximately  one  half  the  acceleration  due  to 

gravity  C73.  This  value  will  be  used  as  the  maximum  value  for 

the  base  disturbances  to  the  test  manipulator.  The  input  will 

I  be  modeled  as  a  random  "white  noise"  input,  with  the 

simulation  returning  a  random  scalar  between  16  and  -16  for 

each  time  step  (see  figure  2-6).  As  is  evident  in  figure  2-6, 

I  the  model  chosen  for  the  base  acceleration  is  indeed  "white 

noise"  with  a  zero  mean.  Random  inputs  in  the  range  of 

acceptable  values  are  generated. 

> 


) 
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M0DEL  0F  BASE  ACCELERAT I 0N 


TIME  ( SEC0NDS ) 


FIGURE  2-6  MODEL  OF  BASE  ACCELERATION 
It  is  important  to  have  an  understanding  of  what  this  input 
entails,  and  to  understand  the  bandwidth  of  this  disturbance. 
By  definition,  white  noise  has  a  very  high  bandwidth. 

Bandwidth  is  roughly  proportional  to  the  speed  of  response 
C123.  The  faster  the  speed  of  response,  the  higher  the 
bandwidth.  As  is  indicated  in  figure  2-6,  the  model  that  is 
being  used  for  the  base  acceleration  has  an  extremely  high 
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bandwidth,  i.e.  the  speed  of  response  is  very  fast.  In  the 
creation  of  this  model  a  sampling  time  of  .2  seconds  was  used. 
Observation  of  the  model,  along  with  application  of  the 
sampling  theorem,  indicate  that  the  bandwidth  of  this  model  is 
of  the  order  of  10  hertz  ClOU.  This  fact  becomes  extremely 
important  in  control  design,  and  will  be  addressed  in  more 
detail  later. 

2.4  Specifications  for  system  performance 

As  was  indicated  earlier,  selection  of  a  realistic 
application  for  the  system  design  provides  for  concrete 
specifications  of  system  performance.  These  specif ications 
will  be  used  to  evaluate  the  effectiveness  of  control  design. 
For  control  design,  specifications  are  needed  in  the  areas  of 
response  time,  accuracy  and  stability. 

In  terms  of  response  time,  in  order  for  a  controlled 
system  to  be  considered  effective  its  response  time  must  be  at 
least  as  fast  as  a  human  operator.  Speed  of  response  in 
loading  the  tank  ammunition  is  a  critical  parameter,  the 
quickest  system  must  be  utilized.  A  well  trained  human 
operator  can  extract  the  ammunition  from  the  ammunition  rack, 
pivot,  and  load  the  round  into  the  gun  tube  in  five  seconds. 
Hence,  the  manipulator  must  be  able  to  move  from  its  initial 
position  to  the  desired  position  in  less  than  five  seconds. 


I 
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In  terras  of  accuracy,  the  ammunition  round  must  slide 
into  the  gun  tube.  The  fact  that  the  ammunition  is  tapered  at 
the  top  (  see  figure  2-7)  allows  the  ammunition  to  basically 
guide  itself  into  the  gun  tube  once  the  ammunition  point  is 
inside  the  gun  breach. 


FIGURE  2-7  SHAPE  OF  TYPICAL  TANK  AMMUNITION 


This  allows  for  a  slight  flexibility  in  terms  of  position 

accuracy  for  the  end  effector  of  the  manipulator.  Based  on 

the  size  of  the  gun  tube  breach,  the  flexibility  amounts  to  a 

range  of  plus  or  minus-  3  inches  in  final  end  effector 

position.  In  the  design  of  the  two-link  manipulator  beinu 

studied,  the  end  effector  location  is  a  function  of  both  the 

joint  angles,  0  and  0  (see  figure  2-1).  Thus,  both  angles 
1  2 

must  be  considered  in  determining  position  accuracy.  By 
trigonometric  transformation,  the  range  of  flexibility  in  the 
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endpoint  location  transforms  to  approximately  an  allowable 
deviation  of  plus  or  minus  .05  radians  in  the  steady  state 
values  for  8^  and  ©2  . 

Transient  response  and  stability  are  additional  important 
specifications.  The  manipulator  must  have  reached  a  stable 
configuration  by  the  time  the  ammunition  round  arrives  at  the 
gun  tube.  Wild  oscillations  at  that  point  could  result  in  the 
round  prematurely  detonating  inside  the  gun  turret. 

2 . 5  Summary 

In  this  chapter,  a  model  to  emulate  the  real  system 
performance  was  designed.  A  realistic  application  for  the 
device  was  selected,  and  model  parameters  were  calculated  with 
this  application  in  mind.  The  model  was  kept  as  simple  as 
possible,  with  emphasis  on  concentrating  on  a  specific  aspect 
of  behavior,  response  to  the  base  acceleration.  The  base 
acceleration  itself  was  modeled  in  such  a  manner  as  to  closely 
emulate  that  acceleration  tank  a  moving  tank  would  experience. 
These  model  parameters  will  now  be  used  in  dynamic 
simulations.  The  system  performance  specifications  were 
determined  in  order  to  evaluate  controller  effectiveness. 
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CHAPTER  3 

DEVELOPMENT  OF  THE  EQUATIONS  OF  MOTION 

3.1  Description  of  Lagrangian  technique 
The  next  step  is  to  develop  the  equations  of  motion  for  the 
model  described  in  the  previous  chapters.  In  this  thesis  the 
Lagrangian  formalism  is  utilized  in  developing  these 
equations.  The  Lagrangian  formalism  is  based  upon  variational 
mechanics,  where  work  and  energy  stored  into  the  system  must 
be  identified.  Workless  forces  such  as  holonomic  constraint 
forces  at  each  of  the  joints  do  not  appear  in  the  formulation. 
The  equations  of  motion  are  written  in  terms  of  generalized 
coordinates  and  generalized  forces.  The  system  to  be 
investigated  is  as  shown  in  figure  3-1: 


FIGURE  3-1  SYSTEM  REPRESENTATION 
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3.2  Use  of  absolute  coordinate  system 


Note  the  use  of  absolute  generalized  coordinates  for  the 
joint  angles,  8^and  0^ , shown  in  figure  3-1.  Both  angles  are 
measured  relative  to  an  inertial  reference  frame.  This  is  an 
important  point.  Usually  manipulator  joint  angles  are 
expressed  in  terms  of  relative  angles,  i.e.  the  angles  are 
measured  relative  to  the  link  to  which  they  are  attached. 
This  is  primarily  due  to  the  fact  that  position  encoders  work 
with  relative  angles.  Absolute  coordinates  were  chosen  in 
this  thesis  due  to  the  fact  that  the  resulting  equations  of 
motion  are  simpler  (less  terms)  C8D. 

3.3  Development  of  equations  of  motion 

The  Lagrangian  formalism  is  based  on  the  equation: 


3  T 

3  qi 

dt 


<$  T 

3  qi 


6  v 
^  q. 


where:  T  =  kinetic  energy 

V  =  ootential  energy 

q  =  generalized  coordinates  ( 6  and  9  ) 
i  1  2 

Q  -  generalized  forces 
i 


(3.1) 


In  this  case  the  generalized  coordinates  are  9  and  0 
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as  shown  in  figure  3-1.  Initially  one  must  evaluate 


the  system's  energy  terms: 


KINETIC  ENERGY 


The  kinetic  energy  for  the  entire  system  is  the  sum  of 
the  kinetic  energy  of  each  of  the  two  links.  Knowing  that  the 
expression  for  the  kinetic  energy  cf  a  rigid  body  is: 

2  2 

T  =  1/2  m  v  +  1/2  J  w  (3.2) 


where:  m  =  the  mass  of  the  body 

v  =  the  linear  velocity  of  the  body  (center  of  mass) 
J  =  the  body's  mass  moment  of  inertia 
w  =  the  angular  velocity  of  the  body  (internal) 


The  system's  kinetic  energy  can  now  be  expressed  in  the 
following  manner: 

o  2  .2  o  2  .2 

T  =  1/2  m  |  v_  |  +  1/2  J  9  +  1/2  m  |  v_  j  +  1/2  J  ©  (3.3) 

1  cl  11  2  c2  2  2 


where : 

o 

j  v_  i  -  the  magnitude  of  the  velocity  of  center  mass  of 
cl  link  one  with  respect  to  inertial  reference 
frame 

o 

|  v_  |  =  the  magnitude  of  the  velocity  of  center  mass  of 
c2  link  two  with  respect  to  inertial  reference 
f  rame 


m  =  the  mass  of  link  1 
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m  =  the  mass  of  link  2 
2  (with  or  without  the  payload) 


J  =  the  mass  moment  of  inertia  of  link  1 
1 

J  =  the  mass  moment  of  inertia  of  link  2 
2  (with  or  without  the  payload) 

Determing  the  components  of  the  velocity  vectors  for  each 
of  the  two  links  is  illustrated  below: 


FIGURE  3-2  SYSTEM  VELOCITY  COMPONENTS 
As  depicted  in  figure  3-2,  the  velocity  components  can 

now  be  expressed  as: 
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o 

V 

cl 


L  0  sin  9  +19  sin  9 

11  1  2  2  2 

(3.5) 

y  +  L  0  cos  0+18  cos  0 
11  12  2  2 


The  magnitude  of  the  velocity  components  can  now  be 


substituted  back  into  equation  (3.4)  to  arrive  at  an 


expression  for  the  system's  total  kinetic  energy. 


POTENTIAL  ENERGY 


The  potential  energy  of  the  system  is  due  to  gravity  and  ca 
be  found  from  the  well  known  relationship: 


V  =  m  g  h  (3.6) 

i  i 


where : 

m  =  mass  of  link  i 
i 


h  =  height  of  the  center  of  mass  of  link  i  above 
i  the  reference  frame 


1  9  sin  9 
11  1 

(3.4) 

•  • 

y  +  1  0  cos  9 
11  1 
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An  examination  of  the  geometry  indicated  in  figure  3-1 
leads  to  the  following  expression  for  the  system's 

potential  energy: 

V=m  g  y  +  m  g  1  sin  0  +  mgy  +  mgL  sin  9  +  m  g  1  sin  0 
1  11  12  21  122  2 

(3.7) 

COMPUTATION  OF  GENERALIZED  FORCES 

The  generalized  forces  are  the  non-conservative  forces  acting 
within  the  system  C31J.  In  this  case  the  non-conservative 
forces  are  the  torques  applied  to  the  actuators  as  shown 
below: 


FIGURE  3-3  GENERALIZED  FORCES 
Analysis  of  the  above  illustration  leads  to: 
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Q  =  T  (3.9) 

2  2 


Reference  C183  contains  a  detailed  description  of  the 
relationship  between  the  generalized  forces.  The  task  is  now 
to  substitute  the  energy  and  generalized  forces  terms  back 
into  equation  3.1,  and  take  the  successive  derivatives  as 
indicated.  The  result  is  the  following  equations  of  motion 
for  the  system: 

2  2.. 

(ml  +  J  +  m  L  )  0  +mLl  cos  (6-0)0 
11  121  1  212  212 

+  (m  1  cos  0  m  L  cos  0  )  y  -  ml  vs  in  06 

1  1  -*■  —L  1  ii  1  i 

.  2 

-  in  L  1  sin  (0-0)0  +  (m  1  cos  0  +  ra  L  cos  0  )  g 

2  12  2  12  11  12  1  1 

=  T  -  T  (3.10) 
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m  L  1  cos  (  0  -  0  )  8  +  (  m  1  +  J  )  0  +  m  1  cos  0  y 

212  211  22  2222  2 

.  2 

+  m  L  1  sin  (0-0)9  +  m  g  1  cos  0 

212  211  22  2 

=  T  (3.11) 

2 

3.4  Nonlinear  Simulation  of  Equations  of  Motion 

The  resulting  equations  of  motion  are  obviously  highly 
nonlinear.  In  order  to  determine  what  effect  various  base 
accelerations  have  on  the  system  itself  in  the  absence  of 
control  a  nonlinear  simulation  was  conducted.  In  addition,  a 
simulation  at  this  point  allows  for  testing  of  the  model. 
Predictable  simulation  results  leads  to  increased  confidence 
in  the  model.  Various  simulations  were  conducted  to  check 
model  response.  The  nonlinear  simulations  were  conducted  via 
the  SIMNON  package  C73  utilizing  a  fourth  order  Runge-Kutta 
integration  routine.  The  results  of  the  simulations  are 


indicated  below: 
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SIMULATION  #  1 


The  first  simulation  is  conducted  in  the  absence  of  any 
external  forces.  Gravity  is  set  to  zero,  and  no  external 
torques  are  applied.  The  base  is  stationary.  Initial 
conditions  for  the  joint  angles  were: 

0  =  . 5  radians 

1 

(3.12) 

6  =1.0  radians 

2 


The  results  are  as  shown: 


no  external  forces 


0  2  5  5  1  5  10 


TIME  (SECONDS) 


FIGURE  3-4  NONLINEAR  SIMULATION  #  1 
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These  results  are  as  expected.  The  joint  angles  remain 
constant  in  the  absence  of  any  external  forces. 

SIMULATION  #  2 

In  the  second  simulation,  gravitational  forces  were 
considered.  Initial  conditions  for  the  joint  angles  were  the 
same  as  in  the  first  simulation.  The  results  are  indicated 
below: 


EFFECT  OF  GRAVITATIONAL  FORCES 


FIGUkc.  3  -  i.  NONLINLAr  SIMULATION  ft,: 

In  the  presence  of  gravity,  the  manipulator  acts  as  a  double 
pendulum,  swinging  with  anticipated  oscillations.  Due  to  the 

fact  that  the  model  does  not  contain  any  damping  or  friction 
forces,  the  oscillation  continues  indef inately .  These  results 
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1 


were  also  as  expected  and  lead  to  increased  confidence  in  the 
equations  of  motion. 

SIMULATION  #  3 

In  this  simulation  a  constant  base  acceleration  of  16  ft  /  sec2 
is  applied  to  the  system.  All  other  parameters  remain  the 
same  as  in  simulation  2.  The  results  are  indicated  below: 


FIGURE  3-6  NONLINEAR  SIMULATION  #  3 

These  results  are  also  as  anticipated.  Trie  constant  base 

accleration  causes  the  joint  angles  to  fall,  and  the  constant 
buildup  of  the  acceleration  forces  causes  the  manipulator 
joint  angles  to  whirl. 
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SIMULATION  #  4 

In  this  simulation  a  sinusoidal  base  acceleration  (16  *  sin 
3t)  was  applied  to  the  system.  The  system  initial  conditions 
remain  the  same,  and  gravity  is  taken  into  consideration.  The 
results  follow: 


EFFECT  OF  SINUSOIDAL  BASE  ACCELERATION 


FIGURE  3-7  NONLINEAR  SIMULATION  #4 
The  results  are  also  predictable.  The  joint  angles  vary  as  a 
function  of  the  base  acceleration . 


SIMULATION  #5 

This  simulation  is  conducted  to  test  the  importance  of 
manipulator  configuration  in  the  analysis.  The  initial 
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conditions  for  the  joint  angles  were  changed  to: 

0  =1.57  radians 

1 

9  =1.57  radians 

2 

The  manipulator  is  now  in  a  totally  vertical  postion,  with 

both  arms  sticking  straight  up.  A  constant  base  acceleration 
2 

of  16  ft/sec  is  applied,  with  the  following  results: 


EFFECT  OF  MANIPULATOR  CONFIGURATION 


TIME  (SECONDS) 


FIGURE  3-8  NONLINEAR  SIMULATION  #5 

This  simulation  illustrates  the  importance  of  manipulator 
configuration.  The  joint  angles  remain  at  1.57  radians,  as 


was  anticipated,  until  such  a  point  that  the  computation 
roundoff  errors  caused  the  manipulator  to  fall  from  its 
vertical  position  and  become  influenced  by  the  base 
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acceleration. 

As  a  further  check  on  the  accuracy  of  the  equations  of 
motion,  a  check  on  the  system's  kinetic  energy  was  conducted. 
In  the  absence  of  any  external  forces,  the  system's  kinetic 
energy  should  remain  constant.  Numerical  simulations  that 
checked  the  kinetic  energy  of  the  system  were  conducted.  The 
system's  kinetic  energy  did  remain  constant  in  the  absence  of 
external  forces,  which  added  credibility  to  the  equations  of 
motion. 

3.5  Linearization  of  Equations  of  Motion 

As  was  stated  earlier,  the  actual  equations  of  motion  are 
highly  nonlinear.  It  is  possible  to  linearize  these  equations 
about  selected  nominal  operating  points,  and  to  then  design  a 
linear  controller  for  the  linearized  equations  that  will 
insure  system  stability.  In  most  cases,  the  nonlinear  system 
will  be  at  least  assympotically  stable  for  small  deviations 
from  the  selected  operating  points  C93.  The  nonlinear  system 
basically  behaves  like  a  linear  system  in  a  "small 
neighborhood"  about  the  operating  points  C103. 

The  procedure  for  linearization  involved  considering 


small  perturbations  about  selected  operating  points  basea  on 

the  following  definitions: 
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•  . n  r  . 

0  =0  +60  (3.13) 

i  i  i 

..  r  .. 

e  =  e  +  o  0 

1  i  i 

n  .  n  .n 

where:  0  ,0  ,0  are  selected  nominal  values 

i  i  i 


for  each  joint,  and 


<$  -0  ,  <$  0  ,  <5  0 


0  are  small  perturbations 
i  i  i 

about  the  nominal  values 


Since  01,<S  9j  are  small  perturbations  from  selected  nominal 
values,  the  following  assumptions  can  be  made: 

(3.14) 


cos  (  <S  0  )  =  0 
i 


sin  (  6  0  )  =  <5  0 
i  i 


(3.15) 


(  <5  0  )  (  i  8  )  =0 

i  j 


(3.16) 


It  is  important  to  understand  this  linearization  process. 
Nominal  values  are  selected  that  best  represent  system 
performance.  In  terms  of  engineering  analysis  it  is  possible 
to  linearize  about  specific  operating  points  with  the  links 


being  stationary  (i.e. 


•  n  ••  n 

0  =  0  =  0),  or  about  specified 

i  i 


trajectories  with  some  nominal  link  velocity  and  acceleration. 
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The  linearized  equations  of  motion  are  listed  in  Appendix 
B.  The  actual  linearization  process  entails  a  great  deal  of 
mathematical  manipulation.  To  expedite  the  process  a  FORTRAN 
program  (LINEAR)  was  written  that  returns  the  specific 
numerical  coefficients  for  each  of  the  system  variables.  (see 
APPENDIX  B) . 

3.6  State  Space  Representation  of  Equations  of  Motion 

The  linearized  equations  can  now  be  written  in 

state-space  form.  The  equations  will  be  represented  in  the 

following  configuration: 

X.  =  A  x  +  _B  y.  +JT  w  (3.17) 

y_  =  C_  x  +  H  y  (  3.18) 

where:  (1)  x.  represents  a  vector  of  the  states  of  the  system. 

The  states  are  those  variables  that  are  required  to 
completely  represent  the  system  in  a  mathematical 
model. In  this  case: 


T 

X  =Cx,x,x,x,x3  (3.19) 

1  2  3  4  5 

=  [ie,i9,(ie,S0,y] 

11  2  2 

(2)  u  is  a  vector  of  the  inputs  to  the  system. 
In  this  case: 

T 


u  =  C 

u 

,  u  n 

n 

II 

n 

T 

,  T  3 

1  2 


where  T  ,  T  are  the  actuator  torques  for  each  of  the 
1  2 


joints. 
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(3)  w  is  a  vector  of  the  disturbances  to  the 
system.  In  this  case: 

w  =  C  y  3  (3.21) 

(4)  and  JZ. are  matrices  which  contain 
the  actual  dynamics  of  the  system.  Their 
numerical  values  are  determined  by  the 
coefficients  of  the  system's  states  as 
described  in  equation  3.19. 

3 . 7  Summary 

In  this  chapter  the  equations  of  motion  for  the  system 
under  investigation  were  developed  utilizing  a  Lagrangian 
technique.  Nonlinear  simulations  of  the  equations  of  motion 
were  then  conducted  to  test  system  response  and  to  determine 
what  effect  the  base  motion  had  on  the  system  itself.  A 
technique  for  linearizing  the  equations  of  motion  was  then 
developed.  The  linearized  equations  were  then  placed  into 
state  space  form.  At  this  point  the  controllers  can  be 
designed  in  an  attempt  to  acheive  acceptable  response. 


CHAPTER  4 
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APPLICATION  OF  CONVENTIONAL  CONTROL  STRATEGY 

4.1  Discussion  of  PID  control  strategy 

Current  industrial  manipulators  utilize  controllers  that 
are  intentionally  simple,  reliable  and  easy  to  implement.  The 
majority  of  industrial  manipulators  are  controlled  by  constant 
gain  linear  feedback  control  systems  C2D.  Typically,  these 
controllers  attempt  to  control  the  manipulator  by  using  local, 
decoupled  proportional-integral-derivative  (PID)  controllers 
at  each  joint.  They  are  able  to  accomplish  position  control, 
i.e.  they  can  move  to  the  manipulator  to  the  configuration 
that  they  desire.  However  these  controllers  tend  to  ignore 
the  nonlinear  dynamics  of  the  system,  and  hence  tend  to  be 

extremely  sensitive  to  disturbances.  In  addition  the 

performance  of  industrial  controllers  is  also  limited  due  to 
restricted  sensory  capability.  They  utilize  only  position  and 
velocity  sensors.  The  question  at  this  point  becomes  whether 
or  not  these  commer ical ly  available  controllers  would  be 
effective  in  controlling  the  manipulator  mounted  on  a  moving 
base.  If  individual  joint  PID  controllers  are  effective  in 
this  situation,  then  the  entire  problem  can  be  simplified. 

PIT)  control  is  a  combinat  ion  . .  '  i  no:,  ;  ■:  ivn'  i  v 

and  integral  control  action  Cl  23.  The  equation  oi  a 

controller  with  this  combined  action  is  given  by: 

m(t)  =  K  e(ti  +  K  elti  +  K  [  eu)  dt  1 4 .  i  > 

p  d  i' 


..  , 
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where.  -  system  error  (actual  -  desired  values) 

K  *  position  error  gain 
P 

K  =  derivative  error  gain 
d 

K  =  integral  error  gain 
i 


or  the  transfer  function: 


K  + 
P 


Kds+Ki 


which  is  indicated  in  the  following  block  diagram: 


FIGURE  4-1  PID  CONTROL  BLOCK  DIAGRAM 

PID  control  incorporates  the  advantages  and  disadvantages 

proportional,  derivative,  and  integral  control.  1 


proportional  controller  basically  acts  as  an  amplifier.  1 
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derivative  control  provides  for  an  anticipatory  action  and 
hence  enhances  stability  and  reduces  the  tendancy  towards 
oscillation.  However,  the  derivative  control  tends  to  amplify 
noise  signals  and  may  have  a  saturation  effect  on  the 
actuators.  The  integral  control  assures  a  reduced  steady 
state  error  and  enhances  steady  state  tracking.  Integral 
control  tends  to  reduce  stability  and  can  lead  to  an 
oscillatory  response  C123. 

4.2  Development  of  a  linear  PID  controller 

Initially  it  is  important  to  attempt  to  design  a  linear 
PID  controller  that  will  compensate  for  the  motion  of  the 
base.  This  was  accomplished  by  first  selecting  nominal  values 
for  the  linearization  (using  program  developed  in  Chapter  3), 
and  then  design  a  controller  for  the  linearized  system.  The 
nominal  values,  0^ selected  for  this  particular  linearization 
are : 

n 

0  =  .349  radians 

1 

n 

0  -  .698  radians 

2 

.  n  .  n 


6 

--  e 

i. 

rad  / 

see 

i 

2 

..n 

,.n 

z 

0 

=  0  = 

1 

rad  / 

sec 

1 

2 

.  n 

y 

=  1  ft 

/ 

sec 

.,n 

2 

y 

=  1  ft 

/ 

sec 

•*  *.*  *,*  »,*  < 
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These  nominal  values  are  depicted  in  the  following 
illustration: 


FIGURE  4-2  NOMINAL  VALUES  FOR  LINEAR  CONTROLLER 

These  nominal  values  were  selected  because  they  provide 
for  a  realistic  description  or  system  performance.  The 
nominal  joint  .  males  proviao  t  ot  j  realist:-.:  com  lour  at  i  "•ri  or 

the  manipulator  during  its  prescribed  motion,  and  also  lead  to 
an  examination  of  torque  capabilities.  The  nominal  velocity 
and  acceleration  terms  provide  for  link,  motion  that  would  lead 
to  acceptable  cycle  time. 
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Utilizing  the  linearization  procedure  developed  in 
Chapter  3,  along  with  the  program  LINEAR  (Appendix  B) ,  the 
equations  of  motion  (equations  3.10  and  3.11)  are  linearized 
about  the  selected  nominal  values.  The  linearized  equations 
become  t 


i  0  =  -11.79  i  9  +  1.13  6  9  +  10.33  6  0  +  .791  <5  9 
1  112  2 

+  .099  y  -  .464  y  +  .021  T  -  .048  T  (4.2) 

1  2 


i  e  =  16.21  6  q  -  2.43  6  e  -  21. 60  £  e  -  1.03  £  e 
21122 

-  .130  y  +  .251  y  -  .028  T  +  .089  T  (4.3) 

1  2 


These  equations  of  motion  can  now  be  placed  in  state-space 
form.  As  was  described  in  Chapter  3,  the  equations  take 
the  form  of : 


X 

=  A 

x  +  B  u  +  r  W 

(4.4) 

V 

=  £ 

x  +  D  u 

(4.5) 

where 

T 

X 

=  f 

x  ,  x  ,  x  ,  x  ,  x  3 

1  2  3  4  5 

(4.6) 

=  c£ 

6  ,£e  ,Se  ,&e  ,  y  3 
112  2 

T 

u 

-  c 

u  ,  u  3 

(4.7) 

1  2 


T  3 
2 
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w  =  C  y  3  (4.8) 


Using  this  format  the  open  loop  system  state  equations 
become : 


Since  this  PID  controller  does  not  take  into  consideration  the 
motion  of  the  base  <i.e.  it  is  assumed  mat  it  reclevos  rio 

sensory  information  as  to  the  base  motion),  the  controller 
will  be  designed  icrnoring  the  base  motion.  This  is 
accomplished  by  ignoring  the  y  and  y  terms  in  the  equations  of 
motion.  This  transforms  the  system  matrices  to: 
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0 

1 

0 

0 

0 

0 

JJ 

-11.79 

0 

1.13 

0 

10.33 

0 

.  791 
1 

£  » 

.021 

0 

-.048 

0 

16.21 

-2.43 

-21.60 

-1.03 

-.028 

.089 

(4.11) 


0 

0 


0 

1 


0 

0 


0 

0 


The  open  loop  system  transfer  function  is  defined  as  the 
ratio  of  the  Laplace  transform  of  the  output  (response 
function,  ^  (s)  and  0 .,( s )  )  to  the  Laplace  transform  of  the 
input  (driving  function,  T^fs)  and  T2(s))  C123.  In  this  case 
the  system  transfer  function  becomes: 

G  (s)  = 

P 

2  2 

.021  s  +  .0189  s  +  .1644  -.048  s  -  .0406  s  -  .1174 

2  2 

-.028  s  -  .0194  s  +  .0103  .089  s  +  .0161  s  +  .2712 

4  3  2 

s  -  .1  s  +  32.4667  s  +  11.2328  3  +  87.2147 


(4.12) 


This  transfer  function  can  also  be  r eor eaentea 
following  manner: 


g 

11 

G  (s)  = 

P 

g 

21 


g 

12 


g 

22 


the 


(4.13) 
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This  is  the  open  loop  transfer  function,  i.e.  the  transfer 
function  of  the  passive  system  in  the  absence  of  control.  It 
is  important  to  note  that  the  linearized  system  itself  is 
inherently  unstable.  This  is  evident  by  applying  the  Routh 
criterion  to  the  system  transfer  function  C123.  In  addition 
investigation  of  the  open  loop  poles  of  the  system  also 
indicate  instablity.  The  open  loop  poles  of  this  system  are: 

p  =  .2648  +  5.4492  i 
1 

p  =  .2648  -  5.4492  i 
2 

(4.14) 

p  =  -.2148  +  1.6983  i 

3 

p  =  -.2148  -  1.6983  i 

4 

The  last  two  poles  lie  in  the  right  half  side  of  the  complex 
plane  and  hence  are  unstable  poles.  This  inherent  instability 
is  a  result  of  including  the  gravitational  effects  in  the 
equations  of  motion. 

As  was  stated  earlier,  PID  controllers  are  designed  based 
on  the  concept  of  independent  joint  control,  also  refered  to 
as  single  axis  control.  In  other  words,  the  motion  of  link 
one  is  control  ed  by  cont.ro  i  inowv.s  an  pi  i  ed  'o  •  ,v?  firs; 
actuator,  and  the  motion  of  link  two  is  controieo  by  inputs 
applied  to  the  second  actuator.  The  couplina  effect  resultina 
from  the  system  nonlinearities  is  ignored.  Utilizing  this 
approach,  linear  controllers  for  each  of  the  two  joints  are 


Page  56 


now  designed. 


FOR  JOINT  ONE 


Joint  one  has  the  torque  applied  to  the  first  actuator  as 

its  input  and  0^  ,the  angle  of  the  first  link,  as  its  output. 

As  is  depicted  in  the  system  transfer  function  this 

relationship  is  described  by  the  single  input  single  output 

transfer  function,  gll.  For  the  design  of  this  controller, 

gll  acts  as  G  .  The  controller  transfer  function,  G  is  given 
^  c 

by  the  dynamics  of  the  PID  controller: 


K  s  +  K  s  +  K 
p  d  j 


G  =  K  +  Ks+K/s  = 
c  j 

p  d  l 


where:  K  =  proportional  gain 
P 


(4.15) 


K  -  derivative  gain 
d 


K  =  integral  gain 


The  closed  loop  tranfer  function  of  the  system  is  given  by  the 
relation: 


G  G 


1  t-  G  G 


i  ‘l  .  i  b  > 
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Linear  simulations,  varying  the  values  of  the  constant 

gains, were  conducted  in  order  to  design  an  acceptable 

controller.  The  gains  to  be  tested  were  initially  selected  by 

classical  control  theory  and  then  verified  by  simulation.  The 

simulations  were  conducted  by  commanding  the  joint  angles  to 

moved  to  some  point  that  was  within  a  small  perturbation  from 

the  linearized  position.  This  is  an  important  point.  By 

definition,  the  linearized  equations  are  only  valid  for  some 

small  perturbation  around  the  selected  nominal  values.  For 

the  purposes  of  these  simulations  the  pertubation  will  be  no 

greater  that  15  degrees  (.26  radians).  In  the  first 

simulation  relatively  small  gains  (  K  =  K  =  K  =  100)  were 

p  d  i 

chosen.  A  simulation  was  then  conducted,  with  the  following 
results  s 

PID  C0NTR0L  0F  LINK  0NE  WITH  SMALL  GAINS 


.05  • 


0.  5.  10.  16.  20. 

TIME  I  SEC0NDS ) 

FIGURE  4-3  PID  CONTROL  WITH  SMALL  GAINS 
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An  interpretation  of  these  results  leads  to  an  interesting 
insight.  By  utilizing  small  gains  the  final  value  was 
eventually  reached,  but  the  response  time  is  unacceptable.  In 
addition,  oscillations  exist  that  are  unacceptable. 

=  1000  leads  to  the 

following  results: 


Increasing  the  gains  to 
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FIGURE  4-4  FID  CONTROL  WITH  LARGER  TAINS 

These  results  are  more  acceptable.  The  response  time  is 
quicker,  and  less  oscillations  exist.  It  was  found  that  by 
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continuing  to  increase  the  gains,  better  results  could  be 
obtained.  However,  very  large  gains  are  unrealistic  in  actual 
system  implementation,  primarily  due  to  the  interaction  with 
structural  resonances  and  control  computer  sampling  times.  In 
addition,  with  large  gains  actuator  saturation  becomes  a 
problem. 


JOINT  TWO 


The  same  design  procedure  was  utilized  in  designing  a 
controller  that  would  be  used  at  the  second  joint  to  control 
the  positon  of  the  second  link.  In  this  case  g22  becomes  the 
system  transfer  function.  Once  again,  increased  gains 
provided  for  more  acceptable  results.  Using  the  values  of  K 

P 

=K  =K  =  1000  the  following  results  were  obtained: 
d  i 

PIO  C0NTR0L  0F  SECOND  LINK  w I TH0UT  BASE  M0TI0N 


TIME  ( SEC0NDS ) 


FIGURE  4-5  PID  CONTROL  OF  LINK  TWO 
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It  is  important  to  understand  the  ramifications  of 

increasing  the  gains  in  this  PID  controller.  As  was  indicated 

in  the  above  simulations,  increasing  the  gains  led  to  faster 

response  time.  This  response  time,  as  was  stated  in  Chapter 

2,  is  a  function  of  the  bandwidth  of  the  controller.  For 

illustration  purposes,  the  bandwidth  of  the  two  controllers 

simulated  (  i.e.  when  K.  =  100  and  when  K  =  1000)  were 

i  i 

determined.  These  bandwidths  are  as  evidenced  in  the 
following  closed  loop  Bode  plots: 


BODE  PLOT 
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BOOE  PLOT 


FREQUENCY  (mZ> 


FIGURE  4-7  BODE  PLOT  FOR  PID  CONTROLLER  WITH  LARGER  GAINS 


The  bandwidth  is  determined  by  the  range  of  frequencies: 

0  <  w  <  w  t  4  .  i  7  ) 

c 

where  w  is  the  frequency  at  which  the  magnitude  of  the 
c 

closed  loop  frequency  response  is  3  db  below  its  zero 
freauencv  value  T121. 


Ar.  i  nee  re;  r.  -  •  . 

that  the  bandwidth  of  the  controller  with  large  gains  i 
greater  than  that  of  the  controller  designed  with  small  gains 
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In  order  for  a  system  to  follow  arbitrary  inputs  accurately, 
the  system  must  have  a  large  bandwidth.  In  essence,  the 
bandwidth  of  the  controller  must  be  larger  than  the  bandwidth 
of  any  inputs  that  it  is  designed  to  compensate  for.  The 
problem  lies  in  the  fact  that  increase  bandwidth  is  a  result 
of  higher  gains  which  could  result  in  such  problems  as 
actuator  saturation,  excessive  noise,  etc. 

It  is  important  to  reemphasize  the  fact  that  PID  control 
is  based  on  the  concept  of  individual  joint  control.  The 
coupling  dynamics  of  the  system  are  ignored.  In  actual  system 
implementation,  the  dynamics  of  the  actuators  themselves  (i.e. 
motor  inertia,  friction,  etc.)  tend  to  reduce  the  coupling 
dynamics  and  allow  the  PID  controller  to  work  better.  Due  to 
the  fact  that  the  model  used  in  these  simulations  did  not 
contain  any  of  actuator  dynamics,  a  simulation  of  the  entire 
system  under  PID  control  did  not  lead  to  acceptable  results. 

Once  again  disregarding  the  couplina  dynamics,  the 
manipulator  under  independent  joint  PID  control  in  subiect  r.o 
base  motion  in  order  to  see  whether  or  not  the  PID  controller 
could  compensate  for  the  motion  of  the  base.  Focusing  on  the 
controller  designed  for  the  first  actuator  and  "he  t  i r t  link, 
the  manipulator  is  now  subjected  to  base  velocity  and 
acclerations  of  the  magnitude  described  in  preceeding 
chapters.  A  linear  simulation  was  conducted, 
following  results: 


with  the 
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FIGURE  4-8  PID  CONTROL  RESPONSE  TO  BASE  DISTURBANCE 

Clearly,  the  FID  controller  is  unabie  to  compensate  for  trie 
base  motion.  The  random  base  acceleration  led  to  wild 
oscillations  of  the  link,  and  a  steady  state  value  was  not 
obtained.  It  must  be  remembered  that  this  simulation  was 
conducted  in  the  most  simple  reaion.  total Iv  icnor ina  the 
coupling  effects  of  the  dynamics  of  the  manipulator  and  also 
ignoring  all  nonlinear  effects.  As  was  stated  in  Chapter  2, 
the  bandwidth  of  the  base  acceleration  is  extremely  hijh.  The 
PID  controller  bandwidth  was  less  than  that  of  the 
disturbance,  and  hence  the  controller  was  not  able  to 
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compensate  for  the  motion  of  the  base.  It  is  possible  that 
the  controller  bandwidth  could  be  increased  to  the  point  where 
it  was  larger  than  the  bandwidth  of  the  disturbance,  but  this 
would  not  be  practical  with  actual  system  components.  The 
actuators  would  have  to  be  very  strong  to  accept  such  large 
gains,  and  would  therefore  be  extremely  expensive  and  bulky. 

4 . 3  Summary 

It  was  shown  in  this  chapter  that  conventional  control 
strategies  are  not  appropriate  for  a  manipulator  mounted  on  a 
moving  base.  Due  to  the  extremely  high  bandwidth  of  the 
disturbance,  a  realistic  PID  controller  could  not  be  designed 
with  a  bandwidth  high  enough  to  compensate  for  the  motion  of 
the  base.  Actuator  saturation,  along  with  additional  problems 
such  as  noise  and  structural  resonance  frequency  interference, 
limit  the  realistic  gains  that  can  be  applied  to  a  PID 


controller . 


It  was  illustrated  in  the  preceeding  chapter  that 
conventional  control  algorithms  were  not  effective  in  the 
moving  base  problem.  PID  control  was  unable  to  effectively 
compensate  for  the  motion  of  the  base.  This  chapter  will  be 
devoted  to  applying  more  advanced  linear  control  strategies  to 
the  moving  base  problem  in  an  attempt  to  see  if  they  prove  to 
be  more  effective  in  dealing  with  the  moving  base.  First, 
decoupling  control  will  be  investigated  C133.  This  is 
primarily  a  control  algorithm  that  eliminates  the  coupling 
dynamics  of  the  manipulator.  The  results  of  this  portion  will 
be  particularly  interesting  based  on  the  high  degree  of 
coupling  in  the  robotic  manipulator.  Secondly,  Linear 
Quadratic  Regulator  Theory  (LQR)  will  be  applied  to  the  moving 
base  problem  C93.  LQR  control  is  a  state  varible  feedback, 
technique  which  attempts  to  arrive  at  an  optimal  control 
strategy.  Finally,  a  pole  placement  technique  will  be 
examined.  This  technique  attempts  to  achieve  stability  and 
desired  responsed  by  adjusting  the  location  of  the  system's 
closed  loop  eigenvalues.  It  is  important  to  note  cnar  these 
control  strategies  are  ah  base  a  on  an  assumption  ot  iineari'.v 
within  the  system.  All  controllers  designed  in  this  chapter, 
along  with  all  simulations  conducted,  will  be  in  the  linear 
mode . 
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5.2  Decoupling  control 

Decoupling  control  is  a  matrix  manipulation  technique 
that  allows  for  diagonalizing  the  system  transfer  function, 
G(s)  C133.  It  is  based  on  the  following  development: 

Given  the  state-space  description  in  the  form  of : 


x  =  A  x+B  u  (5.1) 

X.  =  £.  x  (5.2) 

let  r(t)  =  reference  signal  (5.3) 

and  the  control  input 

u::-_Kx+-Tr(t)  (5.4) 

d  d 


Substituting  equation  5.4  into  equation  5.1  leads  to 

x_=(A-BK)x  +  BT  (5.5) 

d  d 

which  implies  that: 

Def ining 

A'  =  A  -  BK  (5.6) 

Cl 

B '  =  B  T  :  5  .  ' :  i 

d 

The  closed  loop  system  transfer  function  in  the  Laplace 
domain  is  defined  as  the  ratio  of  the  output  to  the 
input  which  in  this  case  C93: 
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y  (s)  _  -1  _ 

G(s)  = - =  C  (s  I  -  A)  B 

r  ( s ) 


(5.8) 


substituting  in  the  values  for  A/  and  B_'  from  equations 
5.6  and  5.7  into  equation  5.8  leads  to: 


G(s)  =  C  ( SI-A  +  B  K  )  B  T 

d  d 


(5.9) 


In  order  to  have  perfect  decoupling  G(s)  must  be  a 
a  diagonal  matrix  (i.e.  all  zeros  on  any  off-diagonal 
element )C93.  Based  on  equation  5.9  in  order  for  G(s) 
to  be  diagonal , 


(5.10) 


CC  (A  -  B  K  )  B  T  1  for  j  =  0 . n-1 

d  d 

must  also  be  diagonal. 


This  can  be  accomplished  only  by  proper  selection  of 

K  and  T  .  Selection  of  K  and  T  accomplished  in  the 
~d  ~d  — d  d 

following  manner: 


Define  the  integers  d  =  min  f i { C  A  B  =  0,  j=0 . n-13 

i  i 

(5.11) 


C  A  1  B 


let  N  = 


m  x  m 


(5.12) 


hi 

C  A  B 
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The  system  can  be  decoupled  only  If  N_  is  non  singular. 
The  following  matrices  will  then  decouple  the  system: 
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The  above  procedure  is  now  applied  to  the  equations  of 
motion  for  the  system  linearized  about  the  nominal  values  that 
were  used  for  designing  the  PID  controller.  Designing  the 
controller  in  the  absence  of  base  motion  using  the  open  loop 
linearized  system  matrices: 


0  10 

-11.79  1.13  10.33 

0  0  0 

16.21  -2.43  -21.60 


0  0 

.021  -.048 

0  0 

-.028  .089 

(5.14) 

10  0  0 

C.  = 

0  0  10 


0  0 
0  0 


Utilizing  the  previous  technique  leads  to: 


CO  03 


C . 0 2 1  -.0481  implies  t  ha  r  d  -  1 


CO  01 


C  -.028  -.0893  implies  that  d  = 

2 


[c  A  B]  = 
[C  A1  b]  = 

(5.15) 

[c  A  Bj  = 
1 

[C  A  Bl  = 
2 


0 

.791 

1 

-1.03 


1 
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which  leads  to: 


1 

CAB 

1 

1 

.021 

-.048 

CAB 

2 

-.028 

.089 

(5.16) 


T  =  N 
d 


169.5238  91.4286 

53.3333  40.0000 


(5.17) 


C  A 
1 

K  =  2 

d  C  A 
2 


(5.18) 


-516.6286  -30.6095  -223.6762  39.9219 

19.6000  -36.9333  -313.0667  .9867 


A_'  =  A  -  B  K  = 
d 


0  10  0 
0  0  0  0 
0  0  0  1 
0  0  0  0 


(5.19) 


B  T  = 
d 


(5.20) 


The  decoupled  system  transfer  function  .becomes 


G  (s)  = 


(5.21) 
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G  (s) 
P 


s 

0 


0 

2 

s 


4 

s 


The  task  is  now  to  design  an  SISO  controller  for  each  of 
the  single  axis  control  loops.  It  was  found  that  an 
acceptable  controller  proved  to  be  a  compensator  of  the 
form: 


10  (s  +  .1) 


G  (s)  = -  (5.22) 

c  s  +  2 


The  closed  loop  system  transfer  function  then  becomes: 


TF 

cl 


G  gl  1 
c 


1  +  G  gll 
c 


10  s  +  1 

-  15.23) 

3  2 

s  +  2  s  +  10  s  +  1 


A  simulation  was  then  conducted  to  test  the  performance 
of  this  SISO  controller.  Again,  concentrating  on  small 


»v « •*.  *\  •’ . *u-  'V *  .  * .  *\  *  *.  * .  ■ . 
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perturbations  from  the  linearized  positions,  the  following 
results  were  obtained: 
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FIGURE  5-2  DECOUPLED  CONTROL  FOR  LINK  ONE 

These  results  are  acceptable,  given  the  specifications 
outlined  in  Chapter  2.  The  response  time  is  within  5  seconds, 
and  stability  and  accuracy  are  achieved.  Based  on  the 
dynamics  of  the  decoupled  system,  the  controller  for  the 
second  control  loop  would  be  the  same.  A  linear  simulation 
was  now  conducted  for  the  entire  system  to  check  response. 
The  system  has  been  linearized  about  the  nominal  values 
described  for  the  PID  controller,  and  the  joint  angles  were 
commanded  to  go  to  values  within  a  small  perturbation  from 


v>. 
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their  linearized  positions.  The  results  are: 


OEC0UPLEQ  C0NTR0L  WITH0UT  BASE  M0TI0N 


0.  6.  10.  16.  20. 


TIME  ( SEC0NDS ) 


FIGURE  5-3  SYSTEM  DECOUPLING  CONTROL 

These  results  for  the  system  meet  the  specifications  outlined 
for  a  desireable  controller.  The  end  effector  location  is 
within  allowable  tolerances,  and  the  response  time  is 
acceptable.  The  coupling  dynamics  of  the  system  have  been 
damped,  and  the  system  has  been  successfully  decoupled. 

The  problem  now  is  to  subject  this  controller  to  the 
dynamics  of  the  moving  base  to  determine  whether  or  not  it  can 
compensate  tor  the  base  motion.  A  simulation  was  conducted  in 

which  the  system  was  augmented  to  account  for  the  motion  of 
the  base.  This  was  accomplished  by  adding  the  base  motion 
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terms  (y  and  y  )  back  into  the  equations  of  motion.  The 
augmented  system  was  then  disturbed  by  a  base  acceleration  of 
the  magnitude  described  in  Chapter  2.  The  following  results 
were  obtained: 
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0.  5.  10.  16.  20. 

TIME  ( SEC0NDS ) 


FIGURE  5-4  DECOUPLED  CONTROL  RESPONSE  TO  BASE  MOTION 

These  results  are  obviously  not  acceptable.  The  base  motion 
created  large  oscillations  in  the  system  performance,  anci 
stability  was  not  achieved  at  cer  twenty  seconds.  Although 
decoupling  control  effectively  resolved  the  problem  of  the 
high  degree  of  coupling  between  the  two  links,  it.  was  not  ab:-. 
to  compensate  for  the  large  base  disturbances.  It  was  found 
that  the  decoupled  system  was  robust  enough  to  accomodate 
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small  disturbances ,  but  not  those  of  the  magnitude  under 

investigation  here.  The  problem  again,  as  with  the  PID 

controller,  lies  in  the  fact  that  the  bandwidth  of  the 

controller  is  smaller  than  the  bandwidth  of  the  disturbance. 

The  poles  of  the  closed  loop  system  under  decoupled  control 

are  as  indicated  below: 

p  =  -.949  +  2.9843  i 
1 

p  =  -.949  -  2.9843  i 
2 

p  =  -.949  +  2.9843  i 

3 

p  =  -.949  -  2.9843  i 

4 

Investigation  of  the  closed  loop  pole  locations  indicate 
that  the  system  bandwidth  is  now  of  the  order  of  3  hertz. 
This  is  smaller  than  the  bandwidth  of  the  disturbance,  and 
hence  it  is  not  able  to  compensate  for  the  disturbance. 
However,  increasing  the  bandwidth  again  leads  to  increased 
gains  and  possible  actuator  saturation. 

5.3  Linear  Quadratic  Regulator  ( LQR )  Control 

The  next  step  in  the  investiaation  was  to  riesian  a 
controller  utilisinu  linear  quadratic  regulator  meory,  in  an 

attempt  to  see  how  that  particular  control  design  does  in 
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LQR  theory  is  a  state  variable  feedback  technique  that 
assumes  full  state  feedback,  i.e.  all  states  must  be 
available  for  measurement  C93.  The  basic  premise  involves 
choosing  the  control  vector  u  (t)  such  that  a  particular 
performance  index,  J,  is  minimized.  J  is  given  by  the 
equation : 

T  T_  T_ 

J  =  x  (t)  S  x(t>  +  1/2  (x.  2.  X  u  R  u  )dt  (5.24) 

f 

where:  S^is  the  solution  to  the  associated  matrix  Ricatti 

equation  and  ^  is  a  positive  semi -def ini te  weighting  matrix  on 
state  errors  ar i  R_  is  a  positive  definite  weighting  matrix  on 
the  control  effort  C32].  The  control  law  that  minimizes  the 
performance  index  J  is  the  simple: 

u_  =  -JL  x  (5.25) 

where _K_ is  the  time  varying  feedback  matrix  given  by 

_  _-l_T 

X(t)  =  H  B  Si'cl  (5.26) 

It  must  be  noted  that  this  procedure  can  be  used  only  if  CA,C3 
form  a  detectable  pair,  and  CA.BD  form  a  stabilizable  pair.  A 
detectable  pair  is  from  a  system  in  which  any  unobservable 
mode  is  stable.  A  stabilizable  pair  implies  that  any 
uncontrollable  mode  within  the  system  is  stable.  This  control 
law  is  considered  optimal  in  the  sense  that,  it  minimize  the 
control  effort  required  to  keep  the  mean-  square  deviation  of 
the  states  from  the  reference  value  as  small  as  possible. 
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The  LQR  control  is  highly  dependant  upon  the  selection  of 
the  performance  index  and  the  weighting  matrices.  Design  of  a 
desireable  controller  becomes  an  iterative  technique.  Initial 
choices  for  the  and  R  matrices  can  be  made  using  "Bryson's 
Rule",  which  provides  estimates  of  the  weighting  matrices  so 
as  to  limit  the  state  deviation  and  the  control  deflection  to 
some  desired  values  C16D.  Bryson's  rule  provides  for: 

1 


£  =  -  (5.27) 

i  2 


(y  max) 
i 

where  y  is  the  maximum  desired  value  of  the  output  and 


1 


R.  =  -  (5.28) 

i  2 


( u  max ) 
i 

where  u  is  the  maximum  desired  value  of  the  control, 
i 

As  the  values  for  £  increase,  the  gains  increase  which  leads 
to  a  larger  bandwidth  and  faster  response  with  better 
precision.  However,  this  also  leads  to  possible  actuator 
saturations.  Varying  the  values  for  R  has  just  the  opposite 
ef  f  ect . 


Applying  the  theory  to  the  investigation  at  hand ,  tne 
first  task  is  to  select  aporopriate  weiahtina  matrices. 
Utilizing  Bryson  ruin  Leans 


let  y  max  =  9  max  =  2.36  radians 
1  1 
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y 

max 

=  e 

max  =  3 . 

14  radians 

2 

2 

u 

max 

=  T 

max  =  20 

f t-lbs 

1 

1 

u 

max 

=  T 

max  =  20 

f t-lbs 

2 

2 

These  maximum  values  were  selected  based  on 
system's  operation  and  acutator  constraints 
lead  to: 


.  180 
0 
0 
0 


0 

0 

0 

0 


0  0 
0  0 
101  0 
0  0 


.0025  0 

0  .0025 


_  1_T 

K  *  R  B  S  (t) 


=  -.0332  2.6244  1.5137  -2 

-2.5464  -6.7067  -1.0223  7 

The  closed  loop  equation  becomes: 

x  -  <  A  -  B  K  )  x 


(5.29) 


knowledge  of  the 
These  values 


(5.30) 


(5.31) 


(5.32) 

.4037 
.  1092 


A  linear  simulation  of  the  closed  loop  system  is 


r 
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1000 
Q=  0 

0 

0 

0  0 
1000  0 

0  1000 
0  0 

0 

0 

0 

1000 

.0025 
R  =  0 

0 

.  0025 

(5.34) 

X  = 

291.43 
-100.13  - 

655.57 

176.87 

15.54 

387.52 

124.02 

596.97 

(5.35) 

the  response  was  as  indicated  below: 
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FIGURE  5-6  LQR  CONTROL  WITHOUT  BASE  MOTION 


This  is  acceptable  for  the  purposes  of  this  investiaation , 
based  on  the  specifications  outlined  in  Chapter  2.  Now  the 

task  again  becomes  to  subject  this  controller  to  the  base 
accelerations  of  appropriate  magnitude.  The  results  of  the 


simulation  are  as  indicated  below: 
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LQR  C0NTR0L  RESP0NSE  T0  BASE  M0TI0N 


FIGURE  5-7  LQR  CONTROL  RESPONSE  TO  BASE  MOTION 

Obviously  the  controller  was  not  able  to  compensate  for  the 
motion  of  the  base.  Stability  was  never  achieved,  and  wild 
oscillations  are  predominant.  Again  the  problem  is  bandwidth. 
The  poles  for  the  closed  loop  system  under  LQR  control  are 


located  at: 
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These  poles  indicate  a  system  bandwidth  of  the  order  of 
1.1  hertz  t  again  too  low  for  the  bandwidth  of  the  disturbance. 
Increase  gains  again  lead  to  increased  system  bandwidth,  along 
with  the  problems  of  actuator  saturation,  etc. 

5.4  Pole  placement  technique 


Another  possible  approach  to  be  used  in  controlling  a 
manipulator  mounted  on  a  moving  base  is  to  use  a  pole 
placement  technique  C101.  It  is  known  that  it  is  possible  to 
place  the  closed  loop  eigenvalues  (poles)  of  a  system  if  the 
system  matrices  _A  and  B.  form  a  controllable  pair.  In  the  full 
state  feedback  realm,  gains  are  used  to  place  the  poles.  The 
question  becomes  where  to  best  place  to  poles  to  achieve 


FIGURE  5-8  POSSIBLE  POLE  LOCATIONS 
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In  the  case  of  this  investigation  stability  is  a  key  issue, 
but  stability  obtained  at  the  least  possible  cost  (i.e.  small 
gains).  In  addition  a  fast  speed  of  response  is  desireable, 
preferably  with  small  overshoot .The  poles,  for  the  sake  of 
stability,  must  all  lie  in  the  left  half  plane.  In  addition, 
the  farther  left  the  poles  are  the  more  stable  the  system. 
However  large  gains  are  required  to  place  the  poles  far  to  the 
left  and  this  also  leads  to  problems  such  as  those  associated 
with  noise,  actuator  saturation,  unmodeled  structural  dynamics 
and  computer  sampling  time.  The  best  approach  is  placing  the 
poles  is  to  use  the  concept  of  the  dominant  pole  pair.  The 
dominant  pole  pair  is  that  pair  of  poles  closest  to  the 
imaginary  axis.  This  set  of  poles  models  an  ideal  second 
order  system  with  desired  speed  of  response.  The  rest  of  the 
poles  are  then  placed  far  enough  away  as  to  not  interfere  with 
the  dominant  pole  pair,  but  not  so  far  as  to  create  the 
problems  mentioned  above.  The  control  law  then  becomes: 

u_  =  -K  x  (5.36) 
where  K  is  the  matrix  of  gains  used  to  place  the  poles. 

In  applying  this  technique  to  the  investigation  at  hand 
it  is  first  important  to  select  the  desired  pole  locations. 
It  is  important  to  first  study  the  location  of  the  open  loop 
poles.  The  open  ioop  pole  locations  are  as  indicated  below: 
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FIGURE  5-9  OPEN  LOOP  POLE  LOCATIONS 


It  is  important  to  remember  that  in  order  to  achieve  stability 
at  the  least  cost,  only  those  poles  that  need  to  be  moved 
should  be  moved,  and  the  poles  should  be  placed  in  a  location 
so  as  to  recieve  desired  response  with  minimal  gains.  This 
becomes  an  iterative  procedure.  Initially  the  poles  were 
placed  at  the  following  locations: 


p  =  -3.0+  3.0  i 
1 

p  =  -3.0  -  3.0  i 

2  (5.37) 
p  =  -10.0  +  10.0  i 

3 

p  =  -10.0  -  10.0  i 

4 

These  pole  locations  were  chosen  for  the  followincr  reasons: 
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(1)  They  were  all  located  in  the  left  half  plane  which 
guaranteed  a  stable  response. 

(2)  They  provided  for  an  optimal  damping  ratio J  of  .707 
which  minimized  overshoot. 

(3)  Poles  3  and  4  were  placed  far  enough  to  the  left  so 
as  not  to  interfere  with  the  dominant  pole  pair,  but  not  so 
far  as  to  create  extremely  large  gains  and  possible  saturation 
problems . 

The  problem  then  became  how  to  calculate  the  gains 
necessary  to  place  the  poles  in  the  desired  locations.  A 
packaged  program  P0LESYS3  C143  was  used  to  calculate  the 
minimum  gains  required  to  place  the  poles.  (see  Appendix  C). 
The  required  gains  for  this  particular  iteration  became: 

K  =  1988.4  987.43  541.43  235.98  (5.38) 

-576.45  -90.529  2242.2  85.686 

A  linear  simulation  of  the  new  system  was  then  conducted, 
based  on  the  following  criteria: 

A'  =  A  -  B  K  u_  =  -  K_  x  (5.39) 

initial  conditions:  6  =  .349  radians  (5.40) 

1 

0  =  .698  radians 

2 

The  system  was  then  subjected  to  an  input  that  resulted  in  a 
small  perturbation  from  the  nominal  conditions,  with  the 
following  results: 
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P0LEPLACE  C0NTR0L  WITH0UT  BASE  M0TI0N 


0.  2.  4.  6.  8.  10. 


TIME  ( SEC0NDS ) 


FIGURE  5-10  POLE  PLACE  CONTROL  WITHOUT  BASE  MOTION 

For  the  purposes  of  this  investigation,  this  response  is 
acceptable.  The  speed  of  response  is  good,  and  little 
overshoot  exists.  The  system  obviously  is  stable. 

The  question  now  becomes  whether  or  not  this  controller 

can  compensate  for  a  base  acceleration  of  the  magnitude 

described  in  this  problem.  An  additional  simulation  was  then 

conducted,  subjecting  the  system  to  a  base  motion  with  a  base 

2 

acceleration  of  at  most  plus  or  minus  16  ft/sec  .  The  results 


were  as  follow: 
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P0LEPLACE  C0NTR0L  RESP0NSE  T0  BASE  M0TI0N 


TIME  ( SEC0NDS ) 


FIGURE  5-11  POLE  PLACE  CONTROL  RESPONSE  TO  BASE  MOTION 


This  controller  was  not  able  to  compensate  for  the  motion  of 
the  base.  The  disturbance  led  to  a  wildly  oscillating  system 
that  never  reached  a  stable  configuration.  An  attempt  was 
then  made  to  stabilize  this  new  system  by  moving  the  poles 
farther  to  the  left  (and  hence  increasing  required  gains), 
with  the  following  results: 

desired  pole  locations:  p  =  -5.0  +  5.0  i 

1 

p  =  -5.0  -  5.0  i 
2  (5.41) 

d  =-15.0+  15.0  i 
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p  =  -15.0  -  15.0  i 
4 

which  led  to  a  gain  matrix*. 


=  5498.9  1539.50  2429.0  421.970  (5.42) 

-2494.7  -333.23  5702.6  40.342 


and  a  response  as  indicated  below: 


P0LEPL ACE  C0NTR0L  RESPONSE  T0  BASE  M0T  E0N -  2ND  ATTEMPT 


0.  5.  10.  16.  20. 

TIME  I SEC0NDS ) 


FIGURE  5-12  POLE  PLACE  CONTROL  RESPONSE  TO  BASE  MOTION 

2ND  ATTEMPT 

The  increased  gains  led  to  an  improved,  though  still 
unacceptable  response  when  compared  to  the  required 
specifications  for  system  response.  The  oscillations  have 
been  damped,  but  an  acceptable  steady  state  value  is  never 
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obtained.  Again  moving 
led  to: 

desired  pole  locations: 


the  poles,  and  increasing 


p  =  -10.0  +  10.0 

1 

p  =  -10.0  -  10.0 

2 

p  =  -15.0  +  15.0 

3 

p  =  -15.0  -  15.0 

4 


the  gains 


(5.43) 


which  led  to  a  gain  matrix  of : 


11152.0 

-5871.0 


2140.10 

-720.90 


5773.6 

11351.0 


340.78 

-223.63 


(5.44) 


and  a  response  as  indicated  below: 


P0LE  PLACE  C0NTR0L  RESPONSE  T0  BASE  M0TI0N-3RD  ATTEMPT 
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FIGURE  5-13  POLE  PLACE  CONTROL  RESPONSE  TO  BASE  MOTION 

THIRD  ATTEMPT 


Again  the  response  is  better,  but  still  not  acceptable 
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was  found  that,  as  in  the  case  of  the  PID  controller. 


continuing 

to  increase  the 

gains  eventually  led 

to 

an 

acceptable. 

though  unrealistic  response. 

Again 

this 

is 

primarily 

due  to  bandwidth 

constraints . 

Continuing 

to 

increase  the  gains  would  eventually  lead  to  a  controller  whose 
bandwidth  exceeded  that  of  the  disturbance,  but  the  gains 
would  be  so  high  that  actuator  saturation,  etc.  would  become 

a  major  problem. 

\ 

5 . 5  Summary 

In  this  chapter  three  existing  advanced  control 
algorithms  (  decoupling  ,  LQR  and  poleplacement)  were  applied 
to  the  moving  base  problem  in  an  attempt  to  see  if  they  were 
robust  enough  to  compensate  for  the  motion  of  the  base.  It 
was  fou  id  that  although  they  all  exhibited  a  certain  amount  of 
disturbance  rejection  capability,  they  were  not  able  to  reject 
disturbances  of  the  magnitude  under  investigation  in  this 
research.  The  bandwidth  of  the  disturbance  becomes  the 
dominant  factor,  and  realistic  controllers  could  not  be 
designed  that  could  compensate  for  the  base  motion. 
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CHAPTER  6 

NONLINEAR  SIMULATION  OF  POLE  PLACE  CONTROLLER 

6.1  Introduction 

Up  to  this  point  in  the  investigation,  all  simulations 
have  been  conducted  utilizing  linear  equations  investigating 
small  perturbations  around  nominal  operating  values.  It  is 
important  now  to  apply  the  linear  controller  that  proved  to  be 
the  most  effective,  the  pole  place  controller,  to  the 
nonlinear  system  with  the  manipulator  operating  over  its  full 
range  of  motion.  This  will  provide  for  insight  into  the 
actual  system  operation. 

6.2  Nonlinear  simulation  results 

The  pole  place  controller  had  the  best  results  in  its 
attempt  to  compensate  for  the  motion  of  the  base  in  the  linear 
realm.  The  results  did  not  meet  the  system  specifications, 
but  were  the  best  of  those  controllers  designed  and  studied. 
The  linear  pole  place  controller  will  now  be  applied  to  the 
nonlinear  system  with  the  manipulator  traversing  over  its 
entire  range  of  motion.  This  simulation  will  be  conducted 
utilizing  the  packaged  program  SIMNON.  The  SIMNON  codes  that 
were  prepared  for  the  simulation  are  listed  at  APPENDIX  A.  A 
fourth  order  Runge-Katta  integration  routine  was  used  for  the 
integration. 
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Simulations  were  conducted  for  all  of  the  sets  of  gains 

that  were  designed  in  Chapter  5  using  the  pole  place 

technique.  In  the  nonlinear  simulations,  the  manipulator  was 

originally  in  a  conf iguration  where  it  could  feasibly  withdraw 

an  ammunition  round  from  the  ammuntion  rack.  This 

configuration  led  the  the  following  initial  conditions: 
o 

9  =2.36  radians 

1 

o 

9  =1.80  radians 

2 


The  manipulator  was  commanded  to  go  to  a  configuration  where 

it  could  feasibly  load  ammuntion  into  the  gun  tube.  The 

desired  values  for  the  gun  tube  then  became: 

0  =  .95  radians 

Id 

(6.1) 

0  =0.0  radians 

2d 


The  simulations  were  first  conducted  where  the  manipulator  was 
assumed  to  be  mounted  on  a  fixed  base.  The  best  results  were 
obtained  for  that  set  of  gains  that  had  the  smallest  bandwidth 
(i.e.  lowest  gains).  These  were  as  indicated  below: 
desired  pole  locations: 

p  ,p  =  -3.0  +  3.0  i 
1  2 

(6.2) 

p  ,d  =  -10.0  +  10.0  i 
3  "  4 


K  =  1988.4  987.43  541.43  235.98  (6.3) 

-576.45  -90.529  2242.2  85.686 


Page  93 


The  results  of  the  simulation  are  as  indicated  below: 


NONLINEAR  SIMUIATION  WITHOUT  BASE  MOTION 


FIGURE  6-1  NONLINEAR  SIMULATION  WITHOUT  BASE  MOTION 

These  results  are  relatively  good.  The  speed  of  response 
is  within  allowable  specifications,  as  is  the  system 
stability.  However,  in  terms  of  accuracy  there  exists  a 
steady  state  error  that  is  outside  of  allowable  limits.  This 
steady  state  error  could  be  a  function  of  unmodeled  system 
dynamics  such  as  static  offset. 

The  system  is  now  subjected  to  a  base  disturbance.  In 
order  to  investigate  disturbances  with  various  bandwidths . 
sinusoidal  inputs  were  used  in  the  nonlinear  simulations  to 
test  system  response.  In  the  first  simulation,  the  initia: 


CHART 


Page  94 


conditions  and  desired  final  values  as  indicated  in  equation 
6.1.  The  system  is  subject  to  a  base  acceleration  of: 

y  =  16  *  sin  (3  t) 

which  provides  for  a  disturbance  of  the  magnitude  under 
investigation  with  a  bandwidth  of  3  hertz.  The  results  of  the 
simulation  are  as  follows: 


EFFECTS  OF  SINUSOIDAL  MOTION  (SIN  3T) 


TIME  (seconds) 


FIGURE  6-2  NONLINEAR  SIMULATION  WITH  BASE  MOTION 

The  system  was  unable  to  adequately  compensate  for  the 
motion  of  the  base.  The  bandwidth  of  the  disturbance  is 
greater  than  the  bandwidth  of  the  controller. 
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In  the  next  simulation,  the  bandwidth  of  the  disturbance 
was  lowered.  In  this  case: 

y  =  16  *  sin  (t) 

The  results  are  as  follow: 


EFFECTS  OF  SINUSOIDAL  NOTION  (SIN  T) 


TIME  (seconds) 


FIGURE  6-3  NONLINEAR  SIMULATION  WITH  BASE  MOTION 

SMALLER  BANDWIDTH 

Ar,  was  expected,  the  controller  is  better  able  to 
compensate  for  the  disturbance  of  a  iower  bandwidth.  The 
oscillations  are  less  pronounced,  and  overall  system  stability 


is  better. 
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CHAPTER  7 
CONCLUSIONS 

It  has  been  shown  in  this  thesis  that  mounting  a 
manipulator  on  a  moving  base  does  indeed  create  complications 
for  the  dynamics  and  control  of  the  manipulator.  The  motion 
of  the  base  creates  disturbances  that  interfere  with  the 
control  of  the  manipulator.  If  the  bandwidth  of  the 
disturbances  is  greater  than  that  of  the  manipulator 
controller,  then  the  controller  will  not  be  able  to  compensate 
for  the  motion  of  the  base.  If  the  bandwidth  of  the 
disturbance  is  high,  as  was  the  case  in  this  thesis,  then  it 
is  possible  that  an  effective  controller  cannot  be 
realistically  designed  to  compensate  for  the  base  motion.  The 
high  bandwidth  necessary  could  only  be  achieved  by  extremely 
high  gains,  which  in  turn  lead  to  such  problems  as  actuator 
saturation,  interference  with  structural  resonances  of  the 
manipulator,  additional  noise  problems,  etc. 

Potential  solutions  to  the  moving  base  problem  do  exist, 
however.  Knowledge  of  the  base  motion  in  terms  of  sensory 
information  for  the  controller  could  allow  the  controller  the 
capability  to  compensate  for  the  base  motion.  This  sensory 
information  could  either  be  used  in  the  actual  design  of  the 
controller,  or  could  be  dynamically  fed  forward  using  the 
Luh-Walker-Paul  algorithm  so  that  the  torques  that  would  be 
required  to  compensate  for  the  base  motion  could  be  computed 
and  effected. 
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Another  possible  solution  is  the  implementation  of  high 
quality  actuators  that  could  accomodate  the  gains  necessary 
for  a  high  bandwidth  controller.  Today's  state-of-the-art 
technology,  however,  does  not  provide  for  actuators  that  meet 
this  need.  Actuators  that  can  accomodate  high  gains  tend  to 
be  bulky  and  slow  and  would  not  meet  speed  of  response 
specifications . 
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CHAPTER  8 

RECOMMENDATIONS  FOR  FURTHER  RESEARCH 

This  investigation  has  established  the  framework  for 
further  studies  of  the  moving  base  problem.  It  has  verified 
that  a  problem  does  indeed  exist  with  mounting  a  manipulator 
on  a  moving  base.  It  has  also  illustrated  that  conventional 
control  techniques,  along  with  more  advanced  control 
algorithms,  are  not  effective  in  compensating  for  the  motion 
of  the  base . 

As  was  indicated  in  Chapter  1,  research  in  this  area  is 
extremely  sparse.  With  this  thesis  as  a  foundation,  the 
following  areas  should  be  explored: 

1)  A  new  algorithm  must  be  developed  that  will  provide 
for  an  effective  control  design  that  is  able  to  compensate  for 
large  base  accelerations. 

2)  The  model  must  be  expanded  from  the  two  link  planar 
model  to  a  three  dimensional,  multi-degree  of  freedom 
manipulator . 

3)  Modeling  considerations  that  were  ignored  in  this 
study,  such  as  motor  inertia  ,  actuator  dynamics,  etc.  must 
be  taken  into  consideration. 

4)  Detailed  computer  simulations  must  be  conducted  to 
verify  controller  effectiveness. 
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APPENDIX  A 

NON-LINEAR  SIMULATION  ROUTINES 
This  appendix  contains  the  routines  that  were  utilized  to 


perforin  the  nonlinear  simulations.  They  were  used  in 


conjuction  with  the  packaged  program  SIMNON. 
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CONTINUOUS  SYSTEM  MBAS 

"THIS  PROGRAM  CONDUCTS  A  NONLINEAR  SIMULATION  OF  SYSTEM  IN  THE 

"ABSENCE  OF  CONTROLS 

“INPUT  T1  T2  YDD 

STATE  XI  X2  X3  X4  X5 

DER  DX1  DX2  DX3  DX4  DX5 

S1=SIN(X1 ) 

Cl=COS(Xl) 

S2=SIN(X3 ) 

C2=C0S (X3 ) 

S3=SIN(X3-X1 ) 

C3=C0S ( X3-X1 ) 

Ml=9 . 2 
M2=9 . 93 
Ll  =  3 . 0 
L2  =  3 . 0 
L1C=1 . 53 
L2C=  1 . 88 
Jl=6 . 44 
J2=5 . 11 
G=0 . 0 
T1  =  0 . 0 
T2=0 . 0 
YDD= 1 . 0 

A=M1AL1C*L1C-*J1+M2*L1*L1 

B=M2*L1*L2C*C3 

C=M1*L1C*C1+M2*L1AC1 

D=M2AL2C*L1AS3 

E=M1*L1C*C1+M2*L1*C1 

F=M1AI.1C*S1AX5AX2 

H=M2*L2CAL2C+J2 

K=M2AL2C*C2 

R=T1 -T2-C* YDD+D*X4*X4-E*G+F 

S=T2-K*YDD-DAX2*X2-KAG 
DX1 =X2 

DX2= (R*H~B*S ) / ( AAH-BAB) 

DX3  =  X4 

DX4  = ( SAA-BAR) / ( AAH-BAB) 

DX5=YDD 

END 
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CONTINUOUS  SYSTEM  SYST 
INPUT  U1  U2  YDD 
OUTPUT  Y1  Y2  Y3  Y4  Y5 
STATE  XI  X2  X3  X4  X5 
DER  DX1  0X2  DX3  DX4  DX5 
S1=SIN(X1 ) 

C1=C0S(X1) 

S2=SIN(X3 ) 

C2=C0S(X3 ) 

S3=SIN(X3-X1) 

C3=C0S(X3-X1 ) 

Ml=9 . 2 
M2  =  9 . 93 
Ll=3 . 0 
L2=3 . 0 
L1C=1 . 53 
L2C=1 . 88 
Jl=6 . 44 
J2=5. 11 
G=-32 . 2 

A=M1*L1C*L1C+J1+M2*L1*L1 

B=M2*L1*L2CAC3 

C=M1*L1C*C1+M2*L1AC1 

D=M2*L2C*L1*S3 

E=M1*L1C*C1+M2*L1AC1 

F=M1AL1CAS1*X5AX2 

H=M2*L2CAL2C+J2 

K=M2AL2C*C2 

R=U1~U2-C*YDD+D*X4*X4~E*G+F 

S=U2~KAYDD-DAX2^X2-K*G 

DXI=X2 

DX2= ( R*H-B*S ) / ( AAH-BAB ) 
DX3=X4 

DX4= ( S*A-BAR ) / ( AAH-B*B ) 

DX5=YDD 

Y1=X1 

Y2 -X2 

Y3  =X3 

Y4=X4 

Y5=X5 


1  a  oil] 
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DISCRETE  SYSTEM  REGNON 

“  THIS  FILE  CONTAINS  THE  POLE  PLACE  CONTROL  CODE 

INPUT  R1  R2  Y1  Y2  Y3  Y4  Y5 

OUTPUT  U1  U2  YDD 

TIME  T 

TSAMP  TS 

Ul=Rl-(  (K1*Y1)+(K2*Y2)+<K3*Y3)+(K4*Y4)  ) 

U2=R2- ( ( K5* Y1 ) + ( K6*Y2 ) + ( K7* Y3 ) + ( K8* Y4 ) ) 

0MG=3 

YDD=16*SIN(T) 

TS=T+H 
H:  .05 
Kl=1988 . 4 
K2=987 .43 
K3=541 . 43 
K4=235. 98 
K5=-576 .45 
K6=-90 . 529 
K7=2242 . 2 
K8=85. 686 
END 
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CONNECTING  SYSTEM  CREGNON 

“  THIS  FILE  CONTAINS  THE  CONNECTOR  FOR  THE  CONTROLLER /SYSTEM 

R1CREGN0N3= . 95 

R2CREGN0N3=0 

Y1CREGN0N3=Y1CSYST3 

Y2CREGN0N3=Y2CSYST3 

Y3CREGNON3=Y3CSYST3 

Y4CREGN0N3=Y4CSYST3 

Y5CREGN0N3=Y5CSYST3 

U1CSYST3=U1CREGN0N3 

U2CSYST3=(J2CR£GN0N3 

YDDC  SYST3 = YDDCREGN0N3 
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APPENDIX  B 


LINEARIZATION  SPECIFICS 


This  appendix  contains  the  linearized  equations  of  motion 
for  a  two  degree  of  freedom  planar  manipulator.  The 
linearization  procedure  used  was  as  described  in  Chapter  3. 
In  addition,  this  appendix  also  contains  the  FORTRAN  code  for 
the  program  LINEAR  which  was  written  to  expedite  the 
linearization  process. 


The  following  are  the  linearized  equations  of  motion: 


EQUATION  1 


2  2,..  n  n 

T  -T  =(ml  +  m  L  +  J  )o  9  +  (m  L  1  cos  0  cos  0 
1  2  21  21  1  1  212  2  1 

n  n  -  ^  ,n  n,. 

+m  L  1  sin  9  sin  9  )o  9  -  (m  1  y  sin  0)0  0 
212  2  12  11  11 


,n  n  n  n  n,n  , 

+  (2n  L  1  9  cos  9  sin  9  -  2m  L  1  sin  0  cos  9-  9  )o  0 


2  12  2 


2  12 


„n  n  n  ..n  n 

+(m  L  1  0  sin  0  cos  0  -  m  L  1  0  cos  0  sin  0 
2122  2  1  2122  2  1 


1  2 
n 


#n  n.n  .n  ,n  n  n 

-  m  1  y  cos  00+mLl  0  0  sin  0  sin  9 

11  11  21222  2  1 

.n  .n  n  n  n  n 

+m  L  1  0  0  cos  0  cos  0  -  m  1  g  sin  0  -  m  L  g  sin  0 

21222  2  111  121  1 

n  n  n  „n  ,  ,.n  n  n 

-m  1  y  sin  6  -  m  L  sin  0  y  )c  9  +  (m  L  1  0  cos  0  sin  9 

11  121  1  1  2122  2  1 

,.n  n  n  .  n  ,n  n  n 

-  m  L  1  0  sin  9  cos  9  -  m  L  1  9  9  cos  0  cos  9 

2122  2  1  21222  2  i 

.  n  ,  n  n  n  .  n 

-  m  L  1  0  0  sin  0  sin  0  )d  9  +  (m  L  cos  0 

21222  2  1  2  11  1 


n  n  n 

+•  m  L  cos  9  )  y  -(mis  in  99  )y 
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EQUATION  2 


n  n  n  n  f  .. 

T  =  (m  L  1  cos  9  cos  9  +  m  L  1  sin  9  sin  0  )  o  0 

2  212  2  1  212  2  1  1 

2  .n  n  n 

+  (m  1  +  J)o9+(2mL19  sin  0  cos  0 

22  2  2  2121  2  1 

.  n  n  n  ,  .  ..n  n  n 

-  2  m  L  1  0  cos  9  sin  0)o0+(mL10  sin  0  cos  0 
2121  2  1  1  2121  2  1 


n  n  .  n  ,n  n  n 

-  m  L  1  0  cos  Q  sin  9  -  m  L  10  0  sin  9  sin  & 

2121  2  1  21211  2  1 


.  n  ,n  n  n  ,.n  n  n 

-  m  L  1  0  0  cos  0  cos  0)00+  (m  L  1  0  cos  0  sin  0 

21211  2  1  1  2121  2  1 

tn  n  n  #n  n  n 

-  m  L  1  9  sin  0  cos  0  -  m  1  y  sin  0  -  m  g  1  sin  0 

2121  2  122  222  2 

,n.n  n  n  .n.n  n  nr 

-  m  L  1  0  0  cos  0  cos  0  +  m  L  1  0  0  sin  0  sin  0  )  a  0 

21211  2  1  21211  2  1  2 


n 

+  (m  1  cos  0  ) 
2  2  2 


y 
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PROGRAM  LINEAR 

cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 
real  t  (  2 ) ,  td(  2) ,  tdd(  2),z(2),zd(2),  zdd(  2) ,  const ,  ml ,  m2 , 
real  12c, jl , j2 ,xl ,x2 ,  xld ,x2d,xldd,x2dd,yl ,y2 ,ydd,yd 
c  variables  are: 

c  xl=theta  one  nominal 

c  x2=theta  two  nominal 

c  xld=nominal  link  one  velocity 

c  x2d=nominal  link  two  velocity 

c  xldd=norainal  link  one  acceleration 

c  x2dd=nominal  link  two  acceleration 

c  yl=nominal  base  velocity 

c  y2=nominal  base  acceleration 

g=-32 . 2 
ml  =  9 . 2 
m2  =  9 . 93 
11=3.0 
12=3.0 
llc=l . 53 
12c=l . 88 
jl=6 . 44 
j  2  =  5 . 1 1 
xl=2 . 36 
x2=l . 83 
xld=l . 0 
x2d=l .  0 
xldd=l . 0 
x2dd=l . 0 
yl=l .  0 
y2=l . 0 
c 

c  Coefficients  for  equation  #1 

tdd( 1 ) =ml*llc*llc+jl+m2* 11*11 

tdd( 2 ) =m2*ll*12c*cos ( x2 ) *cos ( xl ) +m2*ll*12c*sin(x2 ) *sin( xl ) 
td(l)=-ml*llc*yl*sin(xl) 

td( 2 ) =-2*m2*ll*12c*x2d*sin(x2 ) *cos( xl )+2*m2*ll*12c*x2dArcos(x2 ) 
1  *sin(xl) 

t ( 1 ) =-m2*ll*12c*x2dd*cos ( x2 ) *sin(xl ) +m2*ll*12c*x2dd*sin(x2 ) 

1  *cos  ( xl )  -ml*llc*y2*sin(xl )  -m2*ll*y2*sin(xl) -ml*g*llc*sin(xl ) 

1  -m2*g*ll*sin(xl )-ml*llc*yl*cos(xl >*xld+m2*ll*12c*x2d*x2d 
1  *sin( x2 ) *sin< xl )+m2*llA12c*x2d*x2d*cos( x2 ) *cos(xl ) 
t ( 2 ) =-m2*ll*12c*x2dd*sin(x2  )*cos(xl ) +m2*ll*12c*cos ( x2 ) 

1  *sin(xl ) -m2*ll*12c*x2d*x2d*cos(x2  >*cos(xl ) -m2*ll*12c 
1  *x2d*x2dAs in(  x2  )  *sin (xl ) 

yd=-ml* llc*sin( xl ) Axld 
ydd=ml*llc*cos(xl)+m2*ll*cos(xl) 
type  * , ' coef f icients  for  the  first  equation' 
type  a , ' tdd ( 1 ) = ’ , tdd ( 1 ) , ' tdd ( 2 ) = ' , tdd ( 2 ) 
type  a, ' td( 1 ) = ' ,td( 1 ) , ' td( 2) = ' ,td( 2) 
type  *,'t(l)«',t(l),'t(2)B',t<2) 
type  a  f ' yd= ' , yd , ' ydd= ' , ydd 
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Coefficients  for  equation  #2 

zdd( 1 ) =+m2*ll*12c*cos(x2)*cos(xl )+m2*ll*12c*sin(x2) 

1  *sin(xl) 

zdd( 2) =m2*12c*12c+ j2 

zd(  1 )  =2*m2*ll*12c*xld*sin(x2)*cos(xl) -2*«2*ll*12c*xld 
1  *cos(x2)*sin(xl) 

z ( 1 ) =-m2*ll*12c*xldd*cos ( x2 ) *sin<  xl )  +m2*ll*12c*xldd*sin( x2 ) 
1  *cos ( xl ) -m2*ll*12c*xld*xld*sin(x2) *sin(xl ) -m2* ll*12c 

1  *xld*xld*cos(x2)*cos(xl) 

z( 2) =-m2*ll*12c*xldd*sin( x2)*cos (xl ) +m2*ll*12c*xldd 
1  *cos ( x2 ) *sin( xl ) -m2*12c*g*sin(  x2)  -n»2*12c*y2*sin(x2 ) 

1  -m2*ll*12c*xld*xld*cos (x2)*cos (xl )+m2*ll*12c*xld 

1  *xld*sin(x2)*sin(xl) 

ydd2=m2*12c*cos(x2) 

type  *, 'coefficients  for  the  second  equation' 

type  * , '  zdd  ( 1 )  = '  ,  zdd  ( 1 ) , '  zdd  ( 2 )  = '  ,  zdd  (  2 ) 

type  * , ' zd ( 1 ) = ' , zd ( 1 ) , ' zd ( 2 ) = ' , zd ( 2 ) 

type  *,'z(i)>',z(l),'z(2)»',z(2) 

type  * , 'yd2= ' ,yd2 , ' ydd2= ' ,ydd2 

stop 

end 
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APPENDIX  C 
POLES YS 3  ROUTINES 


This  appendix  contains  an  example  of  a  P0LESYS3  program 
run  which  was  used  to  place  the  poles  of  this  multi- input, 
multi-output  system. 
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POLESYS  III  :  Optimal  Output  Feedback  Control  Design  usign 
Pole_jplacement  Techniques  and  Minimal  Cain/Maximal  decoupling 
(c)  1984,  Benito  FERNANDEZ  R.  [  M.I.T.  /  M.E.  Department  ] 
Running  at  the  JCF  /  M.I.T.  Date:  2-SEP-8S  Time:  06:24:22 

Job  Title:  FILE  INPUT 

Logical  Unit  Input:  1  Input  File  Name  :  CASE1.DAT 
Logical  Unit  Output:  2  Output  File  Name  :  CASE111.0UT 

Number  of  States  NX:  4 

Number  of  Meassurements  NY:  2 
Number  of  Controls  NU:  2 

Number  of  Perturbations  NW:  0 

State  Feedback  control  Design 
States  Poles_Placement  Design 

A  matrix 

0.00000D+00  0.10000D+01  0.00000D+00  O.OOOOOD+OO 

-0 . 11790D+02  0 . 11300D+01  0.10330D+02  0.79100D+00 

0.00000D+00  0.00000D+00  0.00000D+00  0.10000D+01 

0 . 16210D+02  -0 . 24300D+01  -0.21600D+02  -0.10300D+01 

B  matrix 

0 . OOOOOD+OO  0 . 00000D+00 

0.21000D-01  -0.48000D-01 

0 . 00000D+00  0 . OOOOOD+OO 

-0.28000D-01  0.8900GD-01 

C  matrix 

0 . 10000D+01  O.OOOOOD+OO  O.OOOOOD+OO  O.OOOOOD+OO 

0.00000D+00  O.OOOOOD+OO  0.10000D+01  O.OOOOOD+OO 

D  matrix 

0 . OOOOOD+OO  0 . OOOOOD+OO 

0 .  OOOOOD+OO  0 . OOOOOD+OO 

Desired  Closed-loop  Eigenvalues 

-0 . 10000D+02  0 . 10000D+02 

-0 . 10000D+02  -0 . 10000D+02 

-0.15000D+02  0.15000D+02 

-0 . 15000D+0 2  -0 . 15000D+0 2 

POLESYS  III  >System  Entering  Optimization  Sequence 

Overall  Transformation  Matrix,  T 

0 . 10000D+01  0.00000D+00  O.OOOOOD+OO  O.OOOOOD+OO 

O.OOOOOD+OO  0.10000D+01  O.OOOOOD+OO  O.OOOOOD+OO 

O.OOOOOD+OO  O.OOOOOD+OO  0.10000D+01  O.OOOOOD+OO 

O.OOOOOD+OO  O.OOOOOD+OO  O.OOOOOD+OO  0.10000D+01 


A  matrix 
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s 

i 


O.OOOOOD+OO  0 . 10000D+01  O.OOOOOD+OO  O.OOOOOD+OO 

-0.11790D+02  0 . 11300D+01  0.10330D+02  0.79100D+00 

O.OOOOOD+OO  O.OOOOOD+OO  O.OOOOOD+OO  O.IOOOOD+Ol 

0 . 16210D+02  -0 . 24300D+01  -0.21600D+02  -0.10300D+01 

B  matrix 

0 . 00000D+00  0 . 00000D+00 

0 . 2 1000I>-0 1  -0 . 48000D-0 1 

0 . 00000D+00  0 . OOOOOD+OO 

-0 . 28000D-01  0.89000D-01 

C  matrix 

0 . 10000D+01  O.OOOOOD+OO  O.OOOOOD+OO  O.OOOOOD+OO 

O.OOOOOD+OO  O.OOOOOD+OO  O.IOOOOD+Ol  O.OOOOOD+OO 

D  matrix 

0 . OOOOOD+CO  0 . OOOOOD+OO 

0 . OOOOOD+OO  0 . OOOOOD+OO 

Convergence  criterion  for  estimates 

0.10000D-03 

Convergence  criterion  for  functions 
0 . 10000D-03 

Convergence  criterion  for  gradient 
0.10000D-03 

Number  of  Iterations  allowed,  MIA 
7777 

Machine  precision,  EPSMCH 
0 . 10000D-09 

Machine  precision,  DWARF 
0 . 10000D-09 

POLESYS  III  >Mlnlmal  Gain  State-Feedback  Control  Calculation  Starts 
Initial  E_2_star 


:  o .  iooood+oi 

|  0 . 20000D+01 

:  0 . 30000D+01 

0.40000D+01 
•  0.50000D+01 

0.60000D+01 
;  0.70000D+01 

I  0.80000D+01 


117 

POLESYS  III  >***  CHECK  DIAGNOSTIC  REPORT  *** 

Sum  of  Squares,  SSQ1 
0.10000D-03 

POLESYS  III  >***  SUCCESSFUL  COMPLETION  *** 

POLESYS  III  >Both  Actual  and  Predicted  Relative  Reductions  in  the  sum  of 
POLESYS  III  >squares  are  at  most  SSQ1 

Original  Space  Control  Gain  Matrix 

0.11152D+05  0 . 21401D+04  0.57736D+04  0.34078D+03 

-0.58710D+04  -0.72090D+03  0.113S1D+05  -0.22363D+03 

Feedback  Control  Gain 

0 . 11152D+05  0.21401D+04  0.57736D+04  0.34078D+03 

-0.58710D+04  -0.72090D+03  0.11351D+05  -0.22363D+03 

(Ad  =  A  -  B*CoGaln)  matrix 

O.OOOOOD+OO  0 . 10000D+01  O.OOOOOD+OO  O.OOOOOD+OO 
-0.52779D+03  -0.78415D+02  0.43392D+03  -0.17100D+02 

O.OOOOOD+OO  O.OOOOOD+OO  O.OOOOOD+OO  0.10000D+01 
0.85099D+03  0.12165D+03  -0.87015D+03  0.28415D+02 

Closed-loop  Eigenvalues 

-0 . 15000D+02  0 . 15000D+02 

-0 . 15000D+0  2  -0 . 15000D+0  2 

-0 . 10000D+02  0 . 10000D+02 

-0 . 10000D+02  -0.10000D+02 

Minimal  Control  Gain,  Kx 

0 . 11152D+05  0 . 21401D+04  0.57736D+04  0.34078D+03 

-0.58710D+04  -0.72090D+03  0.11351D+05  -0.22363D+03 

Original  Space  Control  Gain  Matrix 

0 . 11152D+05  0 . 21401D+04  0.57736D+04  0.34078D+03 

-0.58710D+04  -0.72090D+03  0.11351D+05  -0.22363D+03 

Feedback  Control  Cain 

0.11152D+05  0 . 2140 1D+04  0.57736D+04  0.34078D+03 

-0.58710D+04  -0.72090D+03  0.11351D+05  -0.22363D+03 

(Ad  =  A  -  B*CcCain)  matrix 

O.OOOOOD+OO  0 . 10000D+01  0.00000D+00  0.00000D+00 

-0.52779D+03  -0.78415D+02  0.43392D+03  -0.17100D+02 

0.00000D+00  O.OOOOOD+OO  O.OOOOOD+OO  0.10000D+01 
0.85099D+03  0.12165D+03  -0.87015D+03  0.28415D+02 

Closed- loop  Eigenvalues 

-0 . 15000D+02  0 . 15000D+02 


Page  118 


-0.15000D+02  -0.150000+02 

-0 . 10000D+0  2  0 . 10000D+0 2 

-0 . 10000D+0  2  -0 . 10000D+0 2 

OPTIMO: :>F Inal  Control  Gain  Matrix 

0 , 111520+05  0.21401D+04  0.57736D+04  0.34078D+03 

-0 .58710D+04  -0 .720900+03  0.11351D+05  -0.223630+03 

Feedback  Control  Gain 

0 .111520+05  0 .214010+04  0.57736D+04  0.34078D+03 

-0.587100+04  -0.72090D+03  0.11351D+05  -0. 223630+03 

(Ad  =  A  -  B*CoGaln)  matrix 

O.OOOOOD+OO  0.100000+01  0.00000D+00  O.OOOOOD+OO 

-0.52779D+03  -0.78415D+02  0.43392D+03  -0.17100D+02 

O.OOOOOD+OO  O.OOOOOD+OO  O.OOOOOD+OO  0.10000D+01 

0 . 85099D+03  0.121650+03  -0.870150+03  0.28415D+02 

Closed-loop  Eigenvalues 

-0 . 15000D+02  0.150000+02 

-0 . 15000D+02  -0 . 15000D+02 

-0 . 10000D+02  0 . 10000D+02 

-0.10000D+02  -0 . 1Q000D+02 

POLESYS  III  >End  of  POLESYS  III  Run 
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